/**
 * @file:          StepperMotorController.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.06.07
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.06.07,18:19:42
 */
/* Include Files **************************************************************/
#include "StepperMotorController.h"
#include "DefaultParam.h"
#include "stdio.h"
#include "common.h"
#include "CommunicationInterface.h"
#include "string.h"
#include "stdlib.h"

/* Global Variable Define ************************************************************/
const char version[] = "v3.0";

/* Function Declare ***********************************************************/
void StepperMotorController_Config_CmdSystem_Param(struct StepperMotorController *self);

/* Function Define ************************************************************/
uint8 StepperMotorController_Init(struct StepperMotorController *self)
{
    // DBprintf("StepperMotorController_Init\r\r\n");

    self->Config_CmdSystem_Param = StepperMotorController_Config_CmdSystem_Param;
    self->ControlLoop = StepperMotorController_ControlLoop;
    self->SetControlMode = StepperMotorController_SetControlMode;
    self->SetControlAlgorithm = StepperMotorController_SetControlAlgorithm;
    self->Encoder2ElecAngle = StepperMotorController_Encoder2ElecAngle;
    self->Encoder2Pos = StepperMotorController_Encoder2Pos;
    self->Encoder2PosAbs = StepperMotorController_Encoder2PosAbs;
    self->Encoder2Speed = StepperMotorController_Encoder2Speed;
    // self->EncoderCalibration = StepperMotorController_EncoderCalibration;
    self->UdUqOutput = StepperMotorController_UdUqOutput;
    self->DriverEnableSolution = StepperMotorController_DriverEnableSolution;
    self->StallDetect = StepperMotorController_StallDetect;
    self->ProtectionCheckErr = StepperMotorController_ProtectionCheckErr;
    self->ProtectionCheckWarning = StepperMotorController_ProtectionCheckWarning;
    self->ProtectionDetection = StepperMotorController_ProtectionDetection;
    self->KeyScan = StepperMotorController_KeyScan;
    self->ElecZeroOffsetCalibration = StepperMotorController_ElecZeroOffsetCalibration;
    self->ReceiveDataAnalyse = StepperMotorController_ReceiveDataAnalyse;
    self->CommandAnalyse = StepperMotorController_CommandAnalyse;
    self->ClearInitFlag = StepperMotorController_ClearInitFlag;
    self->SystemReboot = StepperMotorController_SystemReboot;
    self->TimTask = StepperMotorController_TimTask;
    self->DriverEnableControl = StepperMotorController_DriverEnableControl;
    self->UpdateParam = StepperMotorController_UpdateParam;
    self->CheckMotorPhaseOrder = StepperMotorController_CheckMotorPhaseOrder;
    self->EncoderCalibrationOfficial_Run = StepperMotorController_EncoderCalibrationOfficial_Run;
    self->AdcCalibration_Run = StepperMotorController_AdcCalibration_Run;
    // self->MotorRippleTableCalibrationTask = StepperMotorController_MotorRippleTableCalibrationTask;
    self->Cmd_help_Async_Task = StepperMotorController_Cmd_help_Async_Task;
    self->Cmd_helpCmd_Async_Task = StepperMotorController_Cmd_helpCmd_Async_Task;
    self->RegisterCmdToTable = StepperMotorController_RegisterCmdToTable;
    self->SystemIdentification_RL = StepperMotorController_SystemIdentification_RL;
    self->SystemIdentification_Ke = StepperMotorController_SystemIdentification_Ke;
    self->SystemIdentification_JB = StepperMotorController_SystemIdentification_JB;
    self->SystemIdentification_Kt = StepperMotorController_SystemIdentification_Kt;
    self->CheckMotorPhaseResistance = StepperMotorController_CheckMotorPhaseResistance;
    self->VersionPrintTask = StepperMotorController_VersionPrintTask;
    self->ZazenModeRun = StepperMotorController_ZazenModeRun;
    self->ZazenModeEnterOrExit = StepperMotorController_ZazenModeEnterOrExit;
    self->SendFormatedArray = StepperMotorController_SendFormatedArray;

    /* config command system param */
    self->Config_CmdSystem_Param(self);

    /* init modules */
    self->init_flag.bit.communication_interface = self->communication_interface.Init(&self->communication_interface);

    /* init mcu */
    self->mcu->InitSystem(self->mcu);
    self->mcu->Init(self->mcu);

    /* system startup delay */
    for (uint16_t i = 0; i < self->param_system_startup_delay_ms; i++)
    {
        self->mcu->DelayMs(self->mcu, 1);
    }

    /* init flash manager */
    self->flash_manager_param->Init(self->flash_manager_param);

    /* read flash param */
    uint16_t n = 0;
    do
    {
        self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "flash_key");
        self->mcu->DelayMs(self->mcu, 10);
        if (self->flash_manager_param->flash_param_valid_key == self->flash_manager_param->param_flash_param_valid_key)
        {
            break;
        }
        n++;
    } while (n < self->param_flash_key_write_retries_num);

    // DBprintf("%d %d\r\n", self->flash_manager_param->flash_param_valid_key, self->flash_manager_param->param_flash_param_valid_key);

    if (n == self->param_flash_key_write_retries_num) /* flash_param_valid_key != param_flash_param_valid_key*/
    {
        self->flash_manager_param->flash_param_valid_key = self->flash_manager_param->param_flash_param_valid_key;

        /* eliminate partical param */
        // self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "elec_offset");
        // self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "elec_offset_err_deg");
        // self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "phase_order_invert");

        // self->flash_manager_param->SaveAllVariableIntoFlash(self->flash_manager_param);
    }
    else
    {
        /* check flash param version */
        union
        {
            uint16_t all;
            struct
            {
                uint8_t minor;
                uint8_t major;
            } byte;
        } flash_param_version_cur, flash_param_version_flash;

        flash_param_version_cur.all = self->flash_manager_param->param_version.number.all;
        self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "flash_param_version");
        flash_param_version_flash.all = self->flash_manager_param->param_version.number.all;
        self->flash_manager_param->param_version.number.all = flash_param_version_cur.all;

        if (flash_param_version_cur.all != flash_param_version_flash.all) /* flash param version are different */
        {
            self->param_use_default_param = 1;
        }
        else
        {
            /* check if flash checksum is correct, otherwise use default param */
            if (self->flash_manager_param->param_checksum_restore_en != 0)
            {
                if (self->flash_manager_param->CheckAllDataValid(self->flash_manager_param))
                {
                    // self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "use_default_param");
                }
                else
                {
                    self->param_use_default_param = 1;
                }
            }
            else
            {
                // self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "use_default_param");
            }
        }

        /* weather to use default param */
        if (self->param_use_default_param != 0)
        {
            self->param_use_default_param = 0;

            if (flash_param_version_cur.byte.major == flash_param_version_flash.byte.major)
            {
                /* eliminate partical param */
                // self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "elec_offset");
                // self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "elec_offset_err_deg");
                // self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "phase_order_invert");
            }

            // self->flash_manager_param->SaveAllVariableIntoFlash(self->flash_manager_param);
            // self->flash_manager_param->LoadAllVariableFromFlash(self->flash_manager_param);
            // TERMINAL("\r\n\r\nFlash param reset ok!\r\n\r\n");
        }
        else
        {
            // self->flash_manager_param->LoadAllVariableFromFlash(self->flash_manager_param);
        }
    }

    PeriodCorrect(self);
    ParamFurtherSetup(self);

    /* Init */
    self->init_flag.bit.mcu = self->mcu->Init(self->mcu);
    self->communication_interface.CheckPassPrint(self->init_flag.bit.mcu,
                                                 self->communication_interface.param_char_cmd_support,
                                                 "---mcu->Init : [ok]\r\n",
                                                 "---mcu->Init : [failed]\r\n");

    self->init_flag.bit.encoder = self->encoder->Init(self->encoder);
    self->communication_interface.CheckPassPrint(self->init_flag.bit.encoder,
                                                 self->communication_interface.param_char_cmd_support,
                                                 "---encoder->Init : [ok]\r\n",
                                                 "---encoder->Init : [failed]\r\n");

    self->init_flag.bit.driver_board = self->driver_board->Init(self->driver_board);
    self->communication_interface.CheckPassPrint(self->init_flag.bit.driver_board,
                                                 self->communication_interface.param_char_cmd_support,
                                                 "---driver_board->Init : [ok]\r\n",
                                                 "---driver_board->Init : [failed]\r\n");

    self->init_flag.bit.stepper_motor = self->stepper_motor->Init(self->stepper_motor);
    self->communication_interface.CheckPassPrint(self->init_flag.bit.stepper_motor,
                                                 self->communication_interface.param_char_cmd_support,
                                                 "---stepper_motor->Init : [ok]\r\n",
                                                 "---stepper_motor->Init : [failed]\r\n");

    self->init_flag.bit.controller = self->controller->Init(self->controller);
    self->communication_interface.CheckPassPrint(self->init_flag.bit.controller,
                                                 self->communication_interface.param_char_cmd_support,
                                                 "---controller->Init : [ok]\r\n",
                                                 "---controller->Init : [failed]\r\n");

    // DBprintf("%d %d\r\n", flash_param_version_cur, flash_param_version_flash);
    // self->scurve.in_run_start = 0;
    // self->scurve.in_pos_origin = 0;
    // self->scurve.in_pos_target = -20.0;
    // self->scurve.Planning(&self->scurve);

    // self->scurve.Init(&self->scurve);

    // self->init_flag.bit.scurve = self->scurve.Init(&self->scurve);
    // self->communication_interface.CheckPassPrint(self->scurve.Init(&self->scurve),
    //  self->communication_interface.param_char_cmd_support,
    //                                              "---self->scurve.Init : [ok]\r\n",
    //                                              "---self->scurve.Init : [failed]\r\n");

    /* start control period */
    // self->init_flag.bit.adc_calibration = self->driver_board->AdcCalibration(self->driver_board);
    // self->communication_interface.CheckPassPrint(self->init_flag.bit.adc_calibration,
    //  self->communication_interface.param_char_cmd_support,
    //                                              "---self->init_flag.bit.adc_calibration : [ok]\r\n",
    //                                              "---self->init_flag.bit.adc_calibration : [failed]\r\n");

    /* init protection */
    self->protection.Init(&self->protection);

    /* init chopper */
    self->chopper.Init(&self->chopper);

    /* init step_controller */
    self->step_controller.Init(&self->step_controller);

    /* init logger */
    self->logger->Init(self->logger);

    /* init zazen mode */
    self->zazen_mode.trigger_period_count = (uint32_t)(self->zazen_mode.param_trigger_time_s / self->zazen_mode.param_Ts);

    /* ********************** Init value ************************ */
    /* init elec period */
    self->elec_period_pulse = (float)(self->encoder->param_encoder_max_pulse) / (float)(self->stepper_motor->param_pole_pair);
    self->encoder_2_elec_angle = BM_2PI / self->elec_period_pulse;
    self->encoder_2_pos_rad = BM_2PI / (self->encoder->param_encoder_max_pulse);
    self->encoder_2_spd_radps = BM_2PI / (self->encoder->param_encoder_max_pulse * self->controller->param_pos_control_period_s);

    /* init gpio level */
    //    self->mcu->GpioReset(self->driver_board->param_pin_LED.pin);
    //    self->mcu->GpioReset(self->driver_board->param_pin_TestGpio.pin);
    //    self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);
    //    self->mcu->GpioSet(self->driver_board->param_pin_MT_GSENSOR_CS.pin);
    //    self->mcu->GpioReset(self->driver_board->param_pin_CAL_EN.pin); /* 初始化低电平关闭编码器自校准 */

    /* init variable */
    self->encoder_fdb = self->encoder->GetEncoder(self->encoder);
    self->encoder_fdb_last = self->encoder_fdb;
    for (uint16 i = 0; i < BUFFER_SPEED; i++)
    {
        self->encoder_buff[i] = self->encoder_fdb;
    }
    /* 1 */
    self->param_mech_offset = 0; // self->encoder->encoder_pulse_multi_turns;
    self->pos_fdb_encoder = self->Encoder2Pos(self);
    self->step_controller.SetInitStepFromRad(&self->step_controller, self->pos_fdb_encoder);

    // self->in_pos_cmd = self->step_controller.step * self->step_controller.step2rad * self->param_motion_dir;
    self->in_pos_cmd = self->param_motion_dir > 0 ? (self->step_controller.step * self->step_controller.step2rad) : (BM_2PI - self->step_controller.step * self->step_controller.step2rad);

    /* 2 */
    // self->param_mech_offset = self->encoder->encoder_pulse_multi_turns;
    // self->pos_fdb_encoder = self->Encoder2Pos(self);
    // self->in_pos_cmd = self->pos_fdb_encoder;

    /* init leso */
    self->controller->leso.Init_z1z2z3(&self->controller->leso, self->pos_fdb_encoder, 0, 0);

    /* setup preparetion */
    self->mcu->StartTimmer(TRUE);

    /* encoder calibration */
    self->init_flag.bit.encoder_calibration = 1;

    // self->mcu->DelayMs(self->mcu, 1000);

    /* dir processing */
    //    self->step_controller.dir_pin = self->mcu->GpioRead(self->driver_board->param_pin_DIR.pin);
    self->step_controller.dir = (self->step_controller.dir_pin != 0) ? 1 : -1;

    /* init rs485_addr */
    self->rs485_addr = self->communication_interface.param_slave_addr;

    /* enable interrupt */
    self->mcu->EnableInterrupt();
    self->mcu->GpioReset(self->driver_board->param_pin_BUZZER_EN.pin);
    self->mcu->GpioReset(self->driver_board->param_pin_MOTER_PWR_EN.pin);
    self->mcu->GpioSet(self->driver_board->param_pin_LED_GREEN.pin);
    self->mcu->GpioSet(self->driver_board->param_pin_LED_RED.pin);
    self->mcu->GpioSet(self->driver_board->param_pin_LED_STATUS1.pin);
    self->mcu->GpioSet(self->driver_board->param_pin_LED_STATUS2.pin);

    self->mcu->DelayMs(self->mcu, 1000);
    // self->mcu->EnableIWDG(1);
    // self->mcu->EnableWWDG(1);

    // self->driver_board->param_pwm_output_en = 1;
    // TERMINAL("-------------------Initialization complete!-------------------\r\n");
    return TRUE;
}
// extern int16 *data_p;

/* 0 for xyz, 1 for e */

#define BAUD_XYZ_BOOT 230400
#define BAUD_XYZ_APP 230400
#define BAUD_E_BOOT 115200
#define BAUD_E_APP 230400

#define MOTOR_XYZ 1
#define MOTOR_E 0

#define MOTOR MOTOR_XYZ

typedef enum
{
    // 初始化串口
    PROCESS_INIT,
    // 等待开始指令
    PROCESS_Wait_Start,
    PROCESS_Dally_AfterPowerOn,

    // 检查电源电压
    PROCESS_Check_PowerVoltage,

    // 编码器自校准
    PROCESS_SendCmd_JumpApp,
    PROCESS_Check_ReceivePack,
    PROCESS_Cali_EncodeSelf,  // 执行编码器自校准
    PROCESS_Cheak_EncodeSelf, // 检查编码器自校准结果，并断电，设置重新上电前的延迟时间

    // 断电后重新上电
    PROCESS_PowerOn_Second,      // 重新给电机上电，设置上电后的等待时间
    PROCESS_Dally_AfterPowerOn2, // 等待

    // 电角度寻零
    PROCESS_SendCmd_JumpApp2,
    PROCESS_Check_ReceivePack2,
    PROCESS_Cail_ElecZero,
    PROCESS_Check_ElecZero,

    // 闭环运行
    PROCESS_Set_EnPin,
    PROCESS_Dally_AfterEnPin,
    PROCESS_Reset_EnPin,
    PROCESS_Step_Run,
    PROCESS_DelayAfterStepRun,
    PROCESS_Request_ProteckCode,
    PROCESS_Cheak_ProteckCode,

    PROCESS_Success,              // 校准操作成功
    PROCESS_Check_TestResult,     // 检查测试结果
                                  // 写Flash 标志位
    PROCESS_Ready_To_Write_Result, // 写 1
    PROCESS_Read_Previous_Result,          // 写 1
    PROCESS_Write_New_Result,          // 写 1
    PROCESS_Check_FlashWrite,     // 判断写返回
    PROCESS_Read_Flash,           // 读写入的位置
    PROCESS_Check_Flag,           // 判断读出结果

    // 完成状态
    PROCESS_Finish = 98, // 完成
    PROCESS_Over,        // 结束，（成功不成功最终都会到此状态，区别为灯的状态不同）
} Process_Status;

#define Light_RED_LED (self->mcu->GpioReset(self->driver_board->param_pin_LED_STATUS2.pin))

enum
{
    TEST_FAILED = 0,
    TEST_SUCCESS,
} test_result = TEST_FAILED;

// static uint16_t process = 0;
static Process_Status process = PROCESS_INIT;
static float time_s = 0;
static int32_t time_count = 0;
static uint16_t blink_green_led_en;
static uint16_t blink_green_led_count_period = 1;
static uint16_t blink_green_led_count;
static uint16_t blink_red_led_en;
static uint16_t blink_red_led_count_period = 1;
static uint16_t blink_red_led_count;
static uint16_t receive_one_pack_ok_backup;

const uint8_t cmd_jump_app[] = {0xf7, 0xff, 0x04, 0x00, 0x0b, 0x01, 0xc8};              /* 跳转app */
const uint8_t cmd_encoder_cali_official[] = {0xF7, 0x80, 0x04, 0x00, 0x03, 0x00, 0x67}; /* 编码器官方校准 */
const uint8_t cmd_elec_angle_cali[] = {0xF7, 0x80, 0x04, 0x00, 0x04, 0x00, 0x0c};       /* 电零位校准 */
const uint8_t cmd_control_toggle[] = {0xF7, 0x80, 0x04, 0x00, 0x05, 0x04, 0x05};        /* control toggle */
const uint8_t cmd_protect_code_clear[] = {0xF7, 0x80, 0x04, 0x00, 0x0c, 0x05, 0xbf};    /* 清空错误码 */
const uint8_t cmd_protect_code_check[] = {0xF7, 0x80, 0x04, 0x00, 0x0c, 0x0b, 0x95};    /* 查询错误码 */
const uint8_t cmd_get_spd_fdb[] = {0xF7, 0x80, 0x04, 0x00, 0x08, 0x06, 0xe2};           /* 获取速度反馈 */

// -------------------------------|const|motor addr|data length|status|flash| R/W |param index| data(Only W)  Little E | CRC-4 |
const uint8_t cmd_write_clear_test_flag[] = {0xF7, 0x80, 0x09, 0x00, 0x07, 0x05, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x79}; /* Flash 写 protect_warning_code_backup (index 192, type u32)*/
uint8_t cmd_write_test_succeed_flag[] = {0xF7, 0x80, 0x09, 0x00, 0x07, 0x05, 0xc0, 0x01, 0x00, 0x00, 0x00, 0x6F}; /* Flash 写 protect_warning_code_backup (index 192, type u32)*/
uint8_t cmd_write_test_failed_flag[] = {0xF7, 0x80, 0x09, 0x00, 0x07, 0x05, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x79}; /* Flash 写 protect_warning_code_backup (index 192, type u32)*/
const uint8_t cmd_write_read_test_flag[] = {0xF7, 0x80, 0x05, 0x00, 0x07, 0x06, 0xc0, 0xCB};                         /* Flash 读 protect_warning_code_backup */


float spd_fdb;
#define SPD_FDB_INIT_VALUE 999999.0F
static float spd_fdb_max = -SPD_FDB_INIT_VALUE;
static float spd_fdb_min = SPD_FDB_INIT_VALUE;

static float voltage[8] = {0.0};
static uint8_t count = 0, count_judge = 0, print_flag = 0;

void StepperMotorController_ControlLoop(struct StepperMotorController *self)
{

    // enum
    // {
    //     MotorTest_,
    // };
    // const char print_char[] = {0x12, 0x34, 0x56, 0x78, 0x90};//"Status -- count -- judge";

    // if (print_flag >= 1) {
    //     print_flag ++;
    //     if (print_flag > 100) {
    //         Test_UartTxBuff[0] = (char)process;
    //         Test_UartTxBuff[1] = (char)count;
    //         Test_UartTxBuff[2] = (char)count_judge;
    //         ch32v30x_module_v11_UartDMASend_Test((uint8_t*)Test_UartTxBuff, 3);
    //         print_flag = 0;
    //     }
    // }
    // if (Request_Uart1_test() == 1) {
    //     strcpy(Test_UartTxBuff, print_char);
    //     ch32v30x_module_v11_UartDMASend_Test((uint8_t*)Test_UartTxBuff, sizeof(print_char));
    //     print_flag = 1;
    // }

    /* command terminal */
    self->communication_interface.receive_one_pack_ok = self->mcu->UartReceiveOnePackFinished(self->mcu, &self->communication_interface.receive_data_pack.data_len);
    // self->ReceiveDataAnalyse(self);

    /* blink led */
    if (blink_green_led_en)
    {
        if (self->isr_tick % 60000 == 0)
        {
            self->mcu->GpioToggle(self->driver_board->param_pin_LED_GREEN.pin);
        }

        // if (blink_green_led_count == 1 ||
        //     blink_green_led_count == 2)
        // {
        //     if (self->isr_tick % (25000 * 8) == 0)
        //     {
        //         blink_green_led_count = (blink_green_led_count + 1) % (blink_green_led_count_period * 2);
        //         self->mcu->GpioToggle(self->driver_board->param_pin_LED_GREEN.pin);
        //     }
        // }
        // else
        // {
        //     if (self->isr_tick % 25000 == 0)
        //     {
        //         blink_green_led_count = (blink_green_led_count + 1) % (blink_green_led_count_period * 2);
        //         self->mcu->GpioToggle(self->driver_board->param_pin_LED_GREEN.pin);
        //     }
        // }
    }

    if (blink_red_led_en)
    {
        if (self->isr_tick % 60000 == 0)
        {
            self->mcu->GpioToggle(self->driver_board->param_pin_LED_RED.pin);
        }
        // if (blink_red_led_count == 1 ||
        //     blink_red_led_count == 2)
        // {
        //     if (self->isr_tick % (25000 * 8) == 0)
        //     {
        //         blink_red_led_count = (blink_red_led_count + 1) % (blink_red_led_count_period * 2);
        //         self->mcu->GpioToggle(self->driver_board->param_pin_LED_RED.pin);
        //     }
        // }
        // else
        // {
        //     if (self->isr_tick % 25000 == 0)
        //     {
        //         blink_red_led_count = (blink_red_led_count + 1) % (blink_red_led_count_period * 2);
        //         self->mcu->GpioToggle(self->driver_board->param_pin_LED_RED.pin);
        //     }
        // }
    }

    if (self->isr_tick > 5000)
    {

        switch (process)
        {

        case PROCESS_INIT:
            // ch32v30x_module_v11_UartDMASend_Test((uint8_t*)Test_UartTxBuff, sizeof(Test_UartTxBuff));
            process = PROCESS_Wait_Start;
            // self->mcu->GpioReset(self->driver_board->param_pin_LED_STATUS2.pin);    // 亮板子红灯，表示初始状态（未按键、未开始校准）
            break;

        case PROCESS_Wait_Start: /* 等待start按键按下，按下后电机上电 */
            while (!(self->mcu->GpioRead(self->driver_board->param_pin_START.pin) &&
                     self->mcu->GpioRead(self->driver_board->param_pin_STOP.pin)))
            {
                self->mcu->DelayMs(self->mcu, 10);
                if (!self->mcu->GpioRead(self->driver_board->param_pin_START.pin))
                {
                    test_result = TEST_FAILED;
                    self->mcu->GpioReset(self->driver_board->param_pin_BUZZER_EN.pin);
                    self->mcu->GpioSet(self->driver_board->param_pin_MOTER_PWR_EN.pin);
                    self->mcu->GpioSet(self->driver_board->param_pin_LED_GREEN.pin);
                    self->mcu->GpioSet(self->driver_board->param_pin_LED_RED.pin);
                    self->mcu->GpioSet(self->driver_board->param_pin_LED_STATUS1.pin);
                    self->mcu->GpioSet(self->driver_board->param_pin_LED_STATUS2.pin);
                    blink_green_led_count = 0;
                    blink_red_led_count = 0;
                    count_judge = 0;
                    process = PROCESS_Dally_AfterPowerOn;
                    time_s = 2.0; /* 等待boot启动 */
                    time_count = (int32_t)(time_s / 0.000006);
                    // strcpy(Test_UartTxBuff, "Start!!");
                    // ch32v30x_module_v11_UartDMASend_Test((uint8_t*)Test_UartTxBuff, 8);
                    break;
                }
                else if (!self->mcu->GpioRead(self->driver_board->param_pin_STOP.pin))
                {
                    // if ((self->isr_tick % 1000 == 0)) {
                    //     strcpy(Test_UartTxBuff, "Waiting");
                    //     ch32v30x_module_v11_UartDMASend_Test((uint8_t*)Test_UartTxBuff, 8);
                    // }
                }
            }
            break;
        case PROCESS_Dally_AfterPowerOn: /* 上电后等待一段时间 */
            // strcpy(Test_UartTxBuff, "Power On Dally!!");
            // ch32v30x_module_v11_UartDMASend_Test((uint8_t*)Test_UartTxBuff, 17);
            if (time_count < 0)
            {
                process = PROCESS_Check_PowerVoltage;
                count = 0;
            }
            else
            {
                time_count--;
            }
            break;

        case PROCESS_Check_PowerVoltage:
        {
            count++;
            if (count <= 64)
            { // 运行 64 个周期
                if ((count % 8) == 0)
                { // 每 8 个周期 读一次电压值
                    uint8_t index = (count >> 3) - 1;
                    voltage[index] = self->driver_board->GetPowerVoltage(self->driver_board);
                    // ((float *)Test_UartTxBuff)[0] = voltage[index];
                }
            }
            else
            {
                // ch32v30x_module_v11_UartDMASend_Test((uint8_t*)Test_UartTxBuff, 4*8);
                for (int i = 0; i < 8; i++)
                {
                    count_judge += (voltage[i] > 20.0); // 测试，ADC最高识别19.8v, 正常判断大于20V
                }
                if (count_judge == 8)
                {
                    process = PROCESS_SendCmd_JumpApp;
                }
                else
                {
                    test_result = TEST_FAILED;
                    process = PROCESS_Ready_To_Write_Result;
                }
            }
        }
        break;

        case PROCESS_SendCmd_JumpApp: /* 设置波特率，并发指令跳转到app */
#if MOTOR == MOTOR_XYZ
            /* boot波特率 */
            self->mcu->param_uart_baudrate = BAUD_XYZ_BOOT;
            self->mcu->InitUart(self->mcu);
#elif MOTOR == MOTOR_E
            /* boot波特率 */
            self->mcu->param_uart_baudrate = BAUD_E_BOOT;
            self->mcu->InitUart(self->mcu);
#endif
            receive_one_pack_ok_backup = 0;
            /* 发指令跳转app */
            for (size_t i = 0; i < sizeof(cmd_jump_app); i++)
            {
                HexAddByte(cmd_jump_app[i]);
            }
            time_s = 10.0; /* 等待跳转app后的首次写入 */
            time_count = (int32_t)(time_s / 0.000006);
            process = PROCESS_Check_ReceivePack;
            break;
        case PROCESS_Check_ReceivePack: /* 检测跳转app后的第一次数据 */
            if (time_count < 0)
            {
                if (receive_one_pack_ok_backup)
                {
                    if (self->communication_interface.receive_data_pack.data_len > 20)
                    {
                        process = PROCESS_Cali_EncodeSelf;
                    }
                    else
                    {
                        test_result = TEST_FAILED;
                        process = PROCESS_Ready_To_Write_Result;
                    }
                    // process = 4;
                }
                else
                {
                    test_result = TEST_FAILED;
                    process = PROCESS_Ready_To_Write_Result;
                }
            }
            else
            {
                time_count--;
                blink_green_led_en = 1;
                blink_green_led_count_period = 1;
                if (self->communication_interface.receive_one_pack_ok)
                {
                    self->communication_interface.receive_one_pack_ok = 0;
                    receive_one_pack_ok_backup = 1;
                }
            }
            break;
        case PROCESS_Cali_EncodeSelf: /* 切换app波特率，并且发送官方校准指令 */
#if MOTOR == MOTOR_XYZ
            /* boot波特率 */
            self->mcu->param_uart_baudrate = BAUD_XYZ_APP;
            self->mcu->InitUart(self->mcu);
#elif MOTOR == MOTOR_E
            /* boot波特率 */
            self->mcu->param_uart_baudrate = BAUD_E_APP;
            self->mcu->InitUart(self->mcu);
#endif

#if 1
            /* 编码器官方校准 */
            for (size_t i = 0; i < sizeof(cmd_encoder_cali_official); i++)
            {
                HexAddByte(cmd_encoder_cali_official[i]);
            }
#else
            HexAddArray("encoder_calibrate_official", strlen("encoder_calibrate_official"));
#endif
            time_s = 60; /* 等待编码器官方校准结束 */
            time_count = (int32_t)(time_s / 0.000006);
            // time_count = 8000000;
            process = PROCESS_Cheak_EncodeSelf;
            // DBprintf("%d\r\n", process);
            break;
        case PROCESS_Cheak_EncodeSelf: /* 校准完成，检测校准结果，并关闭电机电源 */
            if (time_count < 0)
            {
                self->mcu->GpioReset(self->driver_board->param_pin_MOTER_PWR_EN.pin);
                test_result = TEST_FAILED;
                process = PROCESS_Ready_To_Write_Result;
            }
            else
            {
                time_count--;
                blink_green_led_count_period = 2;

#if 1
                if (self->communication_interface.receive_one_pack_ok)
                {
                    self->communication_interface.receive_one_pack_ok = 0;

                    // self->mcu->GpioReset(self->driver_board->param_pin_LED_STATUS1.pin);

                    if (
                        self->communication_interface.receive_data_pack.databyte[0] == 0xf7 &&
                        self->communication_interface.receive_data_pack.databyte[1] == 0x80 &&
                        self->communication_interface.receive_data_pack.databyte[2] == 0x04 &&
                        // self->communication_interface.receive_data_pack.databyte[3] == 0x00 &&
                        self->communication_interface.receive_data_pack.databyte[4] == 0x03 &&
                        self->communication_interface.receive_data_pack.databyte[5] == 0x0c
                        // self->communication_interface.receive_data_pack.databyte[6] == 0x43
                    )
                    {
                        self->mcu->GpioReset(self->driver_board->param_pin_LED_STATUS1.pin);
                        // self->mcu->GpioReset(self->driver_board->param_pin_LED_GREEN.pin);
                        self->mcu->GpioReset(self->driver_board->param_pin_MOTER_PWR_EN.pin);
                        time_s = 3.0; /* 关掉电机电压，并等待电容放电完毕 */
                        time_count = (int32_t)(time_s / 0.000006);
                        process = PROCESS_PowerOn_Second;
                    }
                    else
                    {
                        test_result = TEST_FAILED;
                        process = PROCESS_Ready_To_Write_Result;
                    }
                }
#endif
            }
            break;
        case PROCESS_PowerOn_Second: /* 重新上电 */
            if (time_count < 0)
            {
                self->mcu->GpioSet(self->driver_board->param_pin_LED_STATUS1.pin);
                self->mcu->GpioSet(self->driver_board->param_pin_MOTER_PWR_EN.pin);
                time_s = 2.0; /* 等待boot启动时间 */
                time_count = (int32_t)(time_s / 0.000006);
                process = PROCESS_Dally_AfterPowerOn2;
                // DBprintf("%d\r\n", process);
            }
            else
            {
                time_count--;
            }
            break;
        case PROCESS_Dally_AfterPowerOn2: /* 上电后等待一段时间 */
            if (time_count < 0)
            {
                process = PROCESS_SendCmd_JumpApp2;
                // DBprintf("%d\r\n", process);
            }
            else
            {
                time_count--;
            }
            break;
        case PROCESS_SendCmd_JumpApp2: /* 切换波特率，并发指令跳转到app */
#if MOTOR == MOTOR_XYZ
            /* boot波特率 */
            self->mcu->param_uart_baudrate = BAUD_XYZ_BOOT;
            self->mcu->InitUart(self->mcu);
#elif MOTOR == MOTOR_E
            /* boot波特率 */
            self->mcu->param_uart_baudrate = BAUD_E_BOOT;
            self->mcu->InitUart(self->mcu);
#endif
            receive_one_pack_ok_backup = 0;
            /* 发指令跳转app */
            for (size_t i = 0; i < sizeof(cmd_jump_app); i++)
            {
                HexAddByte(cmd_jump_app[i]);
            }
            time_s = 2.0; /* 等待第二次boot跳转到app */
            time_count = (int32_t)(time_s / 0.000006);
            process = PROCESS_Check_ReceivePack2;
            // DBprintf("%d\r\n", process);
            break;
        case PROCESS_Check_ReceivePack2: /* 检测跳转app后的第一次数据 */
            if (time_count < 0)
            {
                if (receive_one_pack_ok_backup)
                {
                    if (self->communication_interface.receive_data_pack.data_len > 20)
                    {
                        process = PROCESS_Cail_ElecZero;
                    }
                    else
                    {
                        test_result = TEST_FAILED;
                        process = PROCESS_Ready_To_Write_Result;
                    }
                    // process = 4;
                }
                else
                {
                    test_result = TEST_FAILED;
                    process = PROCESS_Ready_To_Write_Result;
                }
            }
            else
            {
                time_count--;
                blink_green_led_count_period = 3;
                if (self->communication_interface.receive_one_pack_ok)
                {
                    self->communication_interface.receive_one_pack_ok = 0;
                    receive_one_pack_ok_backup = 1;
                }
            }
            break;
        case PROCESS_Cail_ElecZero: /* 切换到app波特率，并且发送电零位校准指令 */
#if MOTOR == MOTOR_XYZ
            /* boot波特率 */
            self->mcu->param_uart_baudrate = BAUD_XYZ_APP;
            self->mcu->InitUart(self->mcu);
#elif MOTOR == MOTOR_E
            /* boot波特率 */
            self->mcu->param_uart_baudrate = BAUD_E_APP;
            self->mcu->InitUart(self->mcu);
#endif

            self->communication_interface.receive_one_pack_ok = 0;
            /* 电零位校准 */
            self->mcu->GpioSet(self->driver_board->param_pin_LED_STATUS1.pin);
            self->mcu->GpioSet(self->driver_board->param_pin_LED_STATUS2.pin);
            self->mcu->GpioSet(self->driver_board->param_pin_LED_GREEN.pin);
            self->mcu->GpioSet(self->driver_board->param_pin_LED_RED.pin);
            for (size_t i = 0; i < sizeof(cmd_elec_angle_cali); i++)
            {
                HexAddByte(cmd_elec_angle_cali[i]);
            }
            time_s = 20.0; /* 电零位调零时间 */
            time_count = (int32_t)(time_s / 0.000006);
            process = PROCESS_Check_ElecZero;
            break;
        case PROCESS_Check_ElecZero: /* 等待返回电零位校准结果 */
            if (self->communication_interface.receive_one_pack_ok)
            {
                self->communication_interface.receive_one_pack_ok = 0;

                data_conversion.u16[0] = ((uint16_t)self->communication_interface.receive_data_pack.databyte[7] << 8) | self->communication_interface.receive_data_pack.databyte[6];
                data_conversion.u16[1] = ((uint16_t)self->communication_interface.receive_data_pack.databyte[9] << 8) | self->communication_interface.receive_data_pack.databyte[8];

                if (
                    self->communication_interface.receive_data_pack.databyte[0] == 0xf7 &&
                    self->communication_interface.receive_data_pack.databyte[1] == 0x80 &&
                    self->communication_interface.receive_data_pack.databyte[2] == 0x0C &&
                    // self->communication_interface.receive_data_pack.databyte[3] == 0x00 &&
                    self->communication_interface.receive_data_pack.databyte[4] == 0x04 &&
                    self->communication_interface.receive_data_pack.databyte[5] == 0x01
                    // data_conversion.f32 < 15.0F
                )
                {
                    process = PROCESS_Set_EnPin;
                }
                else
                {
                    test_result = TEST_FAILED;
                    process = PROCESS_Ready_To_Write_Result;
                }

                // DBprintf("%d\r\n", process);
            }
            if (time_count < 0)
            {
                test_result = TEST_FAILED;
                process = PROCESS_Ready_To_Write_Result;
            }
            else
            {
                blink_green_led_count_period = 4;
                time_count--;
            }
            break;
        case PROCESS_Set_EnPin: /* 使能拉高，并初始化参数 */
            self->mcu->GpioSet(self->driver_board->param_pin_EN.pin);

            time_s = 0.5; /* 使能拉高时间 */
            time_count = (int32_t)(time_s / 0.000006);

            process = PROCESS_Dally_AfterEnPin;
            break;
        case PROCESS_Dally_AfterEnPin: /* 使能拉高持续时间 */
            if (time_count < 0)
            {
                process = PROCESS_Reset_EnPin;
            }
            else
            {
                time_count--;
            }
            break;
        case PROCESS_Reset_EnPin: /* 使能拉高持续时间 */
            self->mcu->GpioReset(self->driver_board->param_pin_EN.pin);

            time_s = 10.0; /* 使能拉高时间 */
            time_count = (int32_t)(time_s / 0.000006);

            spd_fdb_max = -SPD_FDB_INIT_VALUE;
            spd_fdb_min = SPD_FDB_INIT_VALUE;

            process = PROCESS_Step_Run;
            break;
        case PROCESS_Step_Run: /* 发step运动 */
            if (time_count <= (int32_t)(time_s / 0.000006) &&
                time_count > (int32_t)((time_s - 2) / 0.000006))
            {
            }
            else if (time_count < (int32_t)(2 / 0.000006) &&
                     time_count > (int32_t)(0 / 0.000006))
            {
            }
            else if (time_count < 0)
            {
                time_s = 5.0; /* 运动时间 */
                time_count = (int32_t)(time_s / 0.000006);
                self->mcu->GpioSet(self->driver_board->param_pin_EN.pin);

                /* 2:判断速度波动 */
                if (abs_bm(spd_fdb_max - spd_fdb_min) < 80.0F &&
                    abs_bm(spd_fdb_min) > 10.0F &&
                    abs_bm(spd_fdb_max) > 10.0F) /* unit: rad/s，正常波动1rad，标准放宽到5rad/s */
                {
                    process = PROCESS_DelayAfterStepRun;
                }
                else
                {
                    test_result = TEST_FAILED;
                    process = 100;
                }
            }
            else
            {
                if (time_count % 10000 == 0)
                {
                    /* 发指令采集速度 */
                    for (size_t i = 0; i < sizeof(cmd_get_spd_fdb); i++)
                    {
                        HexAddByte(cmd_get_spd_fdb[i]);
                    }
                }
                else
                {
                    if (self->communication_interface.receive_one_pack_ok)
                    {
                        self->communication_interface.receive_one_pack_ok = 0;

                        if (
                            self->communication_interface.receive_data_pack.databyte[0] == 0xf7 &&
                            self->communication_interface.receive_data_pack.databyte[1] == 0x80 &&
                            self->communication_interface.receive_data_pack.databyte[2] == 0x07 &&
                            // self->communication_interface.receive_data_pack.databyte[3] == 0x00 &&
                            self->communication_interface.receive_data_pack.databyte[4] == 0x08
                            // data_conversion.f32 < 15.0F
                        )
                        {
                            data_conversion.u16[0] = ((uint16_t)self->communication_interface.receive_data_pack.databyte[6] << 8) | self->communication_interface.receive_data_pack.databyte[5];
                            data_conversion.u16[1] = ((uint16_t)self->communication_interface.receive_data_pack.databyte[8] << 8) | self->communication_interface.receive_data_pack.databyte[7];
                            spd_fdb = data_conversion.f32;

                            if (spd_fdb > spd_fdb_max)
                            {
                                spd_fdb_max = spd_fdb;
                            }
                            else if (spd_fdb < spd_fdb_min)
                            {
                                spd_fdb_min = spd_fdb;
                            }

                            // /* 2:判断速度波动 */
                            // if (abs_bm(spd_fdb_max - spd_fdb_min) < 6.0F &&
                            //     abs_bm(spd_fdb_min) > 10.0F &&
                            //     abs_bm(spd_fdb_max) > 10.0F) /* unit: rad/s，正常波动1rad，标准放宽到5rad/s */
                            // {
                            //     // process = 8;
                            // }
                            // else if (abs_bm(spd_fdb_min) < (99999999.0F - 1) &&
                            //          abs_bm(spd_fdb_max) < (99999999.0F - 1))
                            // {
                            //     test_result = TEST_FAILED;
                            //     process = 100;
                            // }
                        }
                    }
                }
            }

            if (time_count % 1 == 0)
            {
                self->mcu->GpioToggle(self->driver_board->param_pin_STEP.pin);
            }

            time_count--;
            break;
        case PROCESS_DelayAfterStepRun:
            if (time_count < 0)
            {
                process = PROCESS_Request_ProteckCode;
            }
            time_count--;
            break;
        case PROCESS_Request_ProteckCode: /* 查询保护码，等待返回 */
            for (size_t i = 0; i < sizeof(cmd_protect_code_check); i++)
            {
                HexAddByte(cmd_protect_code_check[i]);
            }
            time_s = 3.0; /* 电零位调零时间 */
            time_count = (int32_t)(time_s / 0.000006);
            process = PROCESS_Cheak_ProteckCode;
            break;
        case PROCESS_Cheak_ProteckCode: /* 没有保护码则通过 */
            if (self->communication_interface.receive_one_pack_ok)
            {
                /* 1 */
                // process = 98;

                /* 2 */
                self->communication_interface.receive_one_pack_ok = 0;

                if (
                    self->communication_interface.receive_data_pack.databyte[0] == 0xf7 &&
                    self->communication_interface.receive_data_pack.databyte[1] == 0x80 &&
                    self->communication_interface.receive_data_pack.databyte[2] == 0x0B &&
                    // self->communication_interface.receive_data_pack.databyte[3] == 0x00 &&
                    self->communication_interface.receive_data_pack.databyte[4] == 0x0c &&
                    self->communication_interface.receive_data_pack.databyte[5] == 0x00 &&
                    self->communication_interface.receive_data_pack.databyte[6] == 0x00 &&
                    self->communication_interface.receive_data_pack.databyte[7] == 0x00 &&
                    self->communication_interface.receive_data_pack.databyte[8] == 0x00
                    // data_conversion.f32 < 15.0F
                )
                {
                    test_result = TEST_SUCCESS;
                    process = PROCESS_Ready_To_Write_Result;
                }
                else
                {
                    test_result = TEST_FAILED;
                    process = PROCESS_Ready_To_Write_Result;
                }
            }
            if (time_count < 0)
            {
                test_result = TEST_FAILED;
                process = PROCESS_Write_Flash;
            }
            else
            {
                blink_green_led_count_period = 4;
                time_count--;
            }
            break;
        case PROCESS_Ready_To_Write_Result:
            time_s = 1.0; /* 写Flash 前 */
            time_count = (int32_t)(time_s / 0.000006);
            process = PROCESS_Write_Flash;
            break;
        case PROCESS_Read_Previous_Result:
            if (time_count < 0)
            {
                for (size_t i = 0; i < sizeof(cmd_write_test_sucess_flag); i++)
                {
                    HexAddByte(cmd_write_test_sucess_flag[i]);
                }
                time_s = 10.0; /* 写完等待 10s 左右检查返回*/
                time_count = (int32_t)(time_s / 0.000006);
                process = PROCESS_Check_FlashWrite;
            }
            else
            {
                time_count--;
            }
            break;
        case PROCESS_Ready_To_Write_Result:
            time_s = 1.0; /* 写Flash 前 */
            time_count = (int32_t)(time_s / 0.000006);
            process = PROCESS_Write_Flash;
            break;
        case PROCESS_Write_Flash: /* 写 Flash 标志 */
            if (time_count < 0)
            {
                for (size_t i = 0; i < sizeof(cmd_write_test_sucess_flag); i++)
                {
                    HexAddByte(cmd_write_test_sucess_flag[i]);
                }
                time_s = 10.0; /* 写完等待 10s 左右检查返回*/
                time_count = (int32_t)(time_s / 0.000006);
                process = PROCESS_Check_FlashWrite;
            }
            else
            {
                time_count--;
            }
            break;
        case PROCESS_Check_FlashWrite:
            if (self->communication_interface.receive_one_pack_ok)
            {
                self->communication_interface.receive_one_pack_ok = 0;

                if (
                    self->communication_interface.receive_data_pack.databyte[0] == 0xf7 &&
                    self->communication_interface.receive_data_pack.databyte[1] == 0x80 &&
                    self->communication_interface.receive_data_pack.databyte[2] == 0x04 &&
                    // self->communication_interface.receive_data_pack.databyte[3] == 0x00 &&
                    self->communication_interface.receive_data_pack.databyte[4] == 0x07 &&
                    self->communication_interface.receive_data_pack.databyte[5] == 0x01)
                {
                    process = PROCESS_Read_Flash;
                }
                else
                {
                    test_result = TEST_FAILED;
                    process = PROCESS_Ready_To_Write_Result;
                }
            }
            if (time_count < 0)
            {
                test_result = TEST_FAILED;
                process = PROCESS_Ready_To_Write_Result;
            }
            else
            {
                time_count--;
            }
            break;

        case PROCESS_Read_Flash:
            for (size_t i = 0; i < sizeof(cmd_write_read_test_flag); i++)
            {
                HexAddByte(cmd_write_read_test_flag[i]);
            }
            time_s = 5.0; /* 发完等待 5s ，判断返回*/
            time_count = (int32_t)(time_s / 0.000006);
            process = PROCESS_Check_Flag;
            break;
        case PROCESS_Check_Flag: /* 检查读回来的是否正确 */
            if (self->communication_interface.receive_one_pack_ok)
            {
                self->communication_interface.receive_one_pack_ok = 0;
                if (
                    self->communication_interface.receive_data_pack.databyte[0] == 0xf7 &&
                    self->communication_interface.receive_data_pack.databyte[1] == 0x80 &&
                    self->communication_interface.receive_data_pack.databyte[2] == 0x07 &&

                    self->communication_interface.receive_data_pack.databyte[4] == 0x07 &&
                    self->communication_interface.receive_data_pack.databyte[5] == 0x01 &&
                    self->communication_interface.receive_data_pack.databyte[6] == 0x00 &&
                    self->communication_interface.receive_data_pack.databyte[7] == 0x00 &&
                    self->communication_interface.receive_data_pack.databyte[8] == 0x00
                    // self->communication_interface.receive_data_pack.databyte[9] == 0x0F &&
                )
                {
                    process = PROCESS_Finish;
                }
                else
                {
                    test_result = TEST_FAILED;
                    process = PROCESS_Ready_To_Write_Result;
                }
            }
            if (time_count < 0)
            {
                test_result = TEST_FAILED;
                process = PROCESS_Ready_To_Write_Result;
            }
            else
            {
                time_count--;
            }
            break;
        case PROCESS_Check_TestResult: /* 判断测试结果是否成功 */
            if (test_result == TEST_SUCCESS)
            {
                self->mcu->GpioReset(self->driver_board->param_pin_LED_GREEN.pin);
                self->mcu->GpioSet(self->driver_board->param_pin_LED_RED.pin);
                blink_green_led_en = 0;
            }
            else if (test_result == TEST_FAILED)
            {
                self->mcu->GpioSet(self->driver_board->param_pin_LED_GREEN.pin);
                self->mcu->GpioReset(self->driver_board->param_pin_LED_RED.pin);
                blink_green_led_en = 0;
            }
            break;
        case PROCESS_Finish:
            self->mcu->GpioReset(self->driver_board->param_pin_LED_GREEN.pin);
            process = PROCESS_Over;
            break;

        case PROCESS_Over: /* 测试结束 */
            blink_green_led_en = 0;
            self->mcu->GpioReset(self->driver_board->param_pin_MOTER_PWR_EN.pin);
            process = PROCESS_Wait_Start;
            break;

        default:
            break;
        }
    }

    /* DBprintf buff send*/
    if (self->logger->trigger == 0)
    {
        // DBprintfDMA_Send();
        // DBprintfDMAAdv_Send();
        self->mcu->DMAHex_Send();
    }

    /* isr tick ++ */
    self->isr_tick++;
}

void StepperMotorController_SetControlMode(struct StepperMotorController *self)
{
}

void StepperMotorController_SetControlAlgorithm(struct StepperMotorController *self)
{
}

float32 StepperMotorController_Encoder2ElecAngle(struct StepperMotorController *self)
{
    float32 elec_angle = 0;
    // elec_angle = fmodf((float)(self->encoder->param_encoder_max_pulse + self->encoder->encoder_pulse_single_turn - self->param_elec_offset), self->elec_period_pulse) / self->elec_period_pulse * BM_2PI;
    elec_angle = fmodf((float)(self->encoder->param_encoder_max_pulse + self->encoder->encoder_pulse_single_turn - self->param_elec_offset), self->elec_period_pulse) * self->encoder_2_elec_angle;

    // DBprintf("%s\r\n", float2str(elec_angle));
    // DBprintf("%d\r\n", self->isr_tick);

    // elec_angle = ((self->encoder->param_encoder_max_pulse + self->encoder->encoder_pulse_single_turn - self->param_elec_offset) % self->elec_period_pulse) * BM_2PI / elec_period;

    // DBprintf("%s ", float2str(fmodf(1.57 , elec_period)/elec_period, 4));
    // elec_angle = (float32)(self->encoder_fdb - self->param_elec_offset) * (float32)self->stepper_motor->param_pole_pair / (float32)self->encoder->param_encoder_max_pulse * BM_2PI;

    return elec_angle;
}

float32 StepperMotorController_Encoder2Pos(struct StepperMotorController *self)
{
    float32 pos = 0;

    // pos = (float32)(self->encoder_fdb - self->param_mech_offset) * BM_2PI / self->encoder->param_encoder_max_pulse;
    pos = (float32)(self->encoder_fdb) * self->encoder_2_pos_rad;

    return pos;
}

float32 StepperMotorController_Encoder2PosAbs(struct StepperMotorController *self)
{
    float32 pos = 0;

    // pos = (float32)(self->encoder_fdb - self->param_elec_offset) * BM_2PI / self->encoder->param_encoder_max_pulse;
    pos = (float32)(self->encoder_fdb - self->param_elec_offset) * self->encoder_2_pos_rad;

    return pos;
}

float32 StepperMotorController_Encoder2Speed(struct StepperMotorController *self)
{
    float32 spd = 0;
    /* raw spd */
    // spd = (float32)(self->encoder_fdb - self->encoder_fdb_last) * self->encoder_2_spd_radps;
    // self->encoder_fdb_last = self->encoder_fdb;

    /* moving average */
    // spd = (float32)(self->encoder_fdb - self->encoder_buff[self->encoder_buff_index]) / self->encoder->param_encoder_max_pulse * BM_2PI / (BUFFER_SPEED * self->controller->param_pos_control_period_s);
    int32 tmp = self->encoder_fdb - self->encoder_buff[self->encoder_buff_index];
    if (tmp > self->encoder->param_encoder_max_pulse >> 1)
    {
        tmp -= self->encoder->param_encoder_max_pulse;
    }
    else if (tmp < -self->encoder->param_encoder_max_pulse >> 1)
    {
        tmp += self->encoder->param_encoder_max_pulse;
    }
    spd = (float32)(tmp) / self->encoder->param_encoder_max_pulse * BM_2PI / (BUFFER_SPEED * self->mcu->param_pwm_period_s);
    self->encoder_buff[self->encoder_buff_index] = self->encoder_fdb;
    self->encoder_buff_index = (self->encoder_buff_index + 1) % BUFFER_SPEED;

    /* first order filter */
    // spd = (float32)(self->encoder_fdb - self->encoder_fdb_last) * self->encoder_2_spd_radps;
    // spd = self->speed_filter.Run(&self->speed_filter, spd);
    // self->encoder_fdb_last = self->encoder_fdb;

    return spd;
}

uint8 StepperMotorController_EncoderCalibration_back(struct StepperMotorController *self)
{
    // int32 *encoder_calibration_buff;
    // int32 encoder_sum = 0;
    // int32 encoder_read = 0;

    // encoder_calibration_buff = malloc(self->param_encoder_cal_count_max * sizeof(int32));

    // DBprintf("Encoder calibration start\r\n");
    // DBprintf("Encoder calibration running......\r\n");

    // self->driver_board->param_pwm_output_en = 1;
    // self->controller->foc.ud = 0;
    // self->controller->foc.uq = 0;
    // self->controller->foc.theta = 0;
    // self->UdUqOutput(self);
    // self->mcu->DelayMs(self->mcu, 500);

    // /* init */
    // self->controller->foc.ud = self->param_ud_cal_set;
    // self->controller->foc.uq = 0;
    // self->controller->foc.theta = 0;
    // self->UdUqOutput(self);
    // self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_step * 2);

    // DBprintf("-------forward start\r\n");
    // /* forward rotation */
    // for (int16 i = 0; i < self->param_encoder_cal_count_max; i++)
    // {

    //     // self->encoder->GetEncoder(self->encoder);
    //     // encoder_read_last = self->encoder->encoder_pulse_single_turn;

    //     self->controller->foc.ud = self->param_ud_cal_set;
    //     self->controller->foc.uq = 0;
    //     self->controller->foc.theta = fmodf(i * (BM_2PI * (float32)self->stepper_motor->param_pole_pair / (float32)self->param_encoder_cal_count_max), BM_2PI);
    //     self->UdUqOutput(self);

    //     self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_step);
    //     // self->encoder->GetEncoder(self->encoder);
    //     encoder_sum = 0;
    //     for (int16 j = 0; j < self->param_encoder_read_count; j++)
    //     {
    //         // self->encoder->GetEncoder(self->encoder);
    //         // encoder_read = self->encoder->encoder_pulse_single_turn;
    //         encoder_read = self->encoder->GetEncoderCalibrated(self->encoder);
    //         // encoder_sum += self->encoder->GetEncoder(self->encoder) % self->encoder->param_encoder_max_pulse;
    //         // encoder_sum += self->encoder->GetEncoder(self->encoder);
    //         // if(encoder_read - encoder_read_last < -(self->encoder->param_encoder_max_pulse / 2))
    //         // {
    //         //     encoder_read += self->encoder->param_encoder_max_pulse;
    //         // }
    //         // else if(encoder_read - encoder_read_last > (self->encoder->param_encoder_max_pulse / 2))
    //         // {
    //         //     encoder_read -= self->encoder->param_encoder_max_pulse;
    //         // }
    //         encoder_sum += encoder_read;
    //         self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_read);
    //     }
    //     encoder_sum = encoder_sum / (int16)(self->param_encoder_read_count);

    //     // if(encoder_sum > self->encoder->param_encoder_max_pulse)
    //     // {
    //     //     encoder_sum -= self->encoder->param_encoder_max_pulse;
    //     // }
    //     // else if(encoder_sum < 0)
    //     // {
    //     //     encoder_sum += self->encoder->param_encoder_max_pulse;
    //     // }
    //     DBprintf("%ld\r\n", encoder_sum);
    //     encoder_calibration_buff[i] = encoder_sum;
    //     // DBprintf("%d %d\r\n", i, encoder_calibration_buff[i]);
    // }

    // DBprintf("-------backward start\r\n");
    // /* backward rotation */
    // for (int16 i = self->param_encoder_cal_count_max - 1; i >= 0; i--)
    // {

    //     // self->encoder->GetEncoder(self->encoder);
    //     // encoder_read_last = self->encoder->encoder_pulse_single_turn;

    //     self->controller->foc.ud = self->param_ud_cal_set;
    //     self->controller->foc.uq = 0;
    //     self->controller->foc.theta = fmodf(i * (BM_2PI * (float32)self->stepper_motor->param_pole_pair / (float32)self->param_encoder_cal_count_max), BM_2PI);
    //     self->UdUqOutput(self);

    //     self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_step);
    //     // self->encoder->GetEncoder(self->encoder);
    //     encoder_sum = 0;
    //     for (int16 j = 0; j < self->param_encoder_read_count; j++)
    //     {
    //         // self->encoder->GetEncoder(self->encoder);
    //         // encoder_read = self->encoder->encoder_pulse_single_turn;
    //         encoder_read = self->encoder->GetEncoderCalibrated(self->encoder);
    //         // encoder_sum += self->encoder->GetEncoder(self->encoder) % self->encoder->param_encoder_max_pulse;
    //         // encoder_sum += self->encoder->GetEncoder(self->encoder);
    //         // if(encoder_read - encoder_read_last < -(self->encoder->param_encoder_max_pulse / 2))
    //         // {
    //         //     encoder_read += self->encoder->param_encoder_max_pulse;
    //         // }
    //         // else if(encoder_read - encoder_read_last > (self->encoder->param_encoder_max_pulse / 2))
    //         // {
    //         //     encoder_read -= self->encoder->param_encoder_max_pulse;
    //         // }
    //         encoder_sum += encoder_read;
    //         self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_read);
    //     }
    //     // encoder_calibration_buff[self->param_encoder_cal_count_max + i] = encoder_sum / (int16)(self->param_encoder_read_count);
    //     encoder_sum = encoder_sum / (int16)(self->param_encoder_read_count);
    //     // if(encoder_sum > self->encoder->param_encoder_max_pulse)
    //     // {
    //     //     encoder_sum -= self->encoder->param_encoder_max_pulse;
    //     // }
    //     // else if(encoder_sum < 0)
    //     // {
    //     //     encoder_sum += self->encoder->param_encoder_max_pulse;
    //     // }
    //     DBprintf("%ld\r\n", encoder_sum);
    //     // if(fabs((encoder_calibration_buff[i] - encoder_sum)) > self->encoder->param_encoder_max_pulse*0.5)
    //     // {
    //     //     encoder_calibration_buff[i] = (encoder_calibration_buff[i] + encoder_sum + ) / 2;
    //     // }
    //     // else
    //     // {
    //     //     encoder_calibration_buff[i] = (encoder_calibration_buff[i] + encoder_sum) / 2;
    //     // }
    //     encoder_calibration_buff[i] = (encoder_calibration_buff[i] + encoder_sum) / 2;

    //     if (encoder_calibration_buff[i] > self->encoder->param_encoder_max_pulse)
    //     {
    //         encoder_calibration_buff[i] -= self->encoder->param_encoder_max_pulse;
    //     }
    //     else if (encoder_calibration_buff[i] < 0)
    //     {
    //         encoder_calibration_buff[i] += self->encoder->param_encoder_max_pulse;
    //     }
    //     // DBprintf("%d %d\r\n", i, encoder_calibration_buff[i]);
    // }

    // self->controller->foc.ud = 0;
    // self->controller->foc.uq = 0;
    // self->controller->foc.theta = 0;
    // self->driver_board->param_pwm_output_en = 0;
    // self->UdUqOutput(self);
    // self->mcu->DelayMs(self->mcu, 500);

    // /* average */
    // for (int16 i = 0; i < self->param_encoder_cal_count_max; i++)
    // {
    //     DBprintf("%d\t %ld \r\n", i, encoder_calibration_buff[i]);
    //     self->mcu->DelayMs(self->mcu, 1);
    // }

    // DBprintf("interpolation ---------------------\r\n");

    // /* interpolation */
    // int32 ticks = 0;
    // int32 stepNo = 0;
    // int32 iStart = 0;
    // int32 jStart = 0;
    // for (int16 i = 0; i < self->param_encoder_cal_count_max; i++)
    // {
    //     DBprintf("----------i:%d ", i);
    //     ticks = encoder_calibration_buff[(i + 1) % self->param_encoder_cal_count_max] - encoder_calibration_buff[i % self->param_encoder_cal_count_max];

    //     if (ticks < -(self->encoder->param_encoder_max_pulse * 0.5))
    //     {
    //         ticks += self->encoder->param_encoder_max_pulse;
    //     }
    //     else if (ticks > (self->encoder->param_encoder_max_pulse * 0.5))
    //     {
    //         ticks -= self->encoder->param_encoder_max_pulse;
    //     }
    //     for (int32_t j = 0; j < ticks; j++)
    //     {
    //         stepNo = (encoder_calibration_buff[i] + j) % self->encoder->param_encoder_max_pulse;
    //         if (stepNo == 0)
    //         {
    //             iStart = i;
    //             jStart = j;
    //             // DBprintf("----------------iStartjStart:%d %d ", iStart, jStart);
    //             // DBprintf("es1,es2:%d, %d ", encoder_calibration_buff[(i + 1) % self->param_encoder_cal_count_max], encoder_calibration_buff[i % self->param_encoder_cal_count_max]);
    //             // DBprintf("\r\n");
    //         }
    //         // DBprintf("ticks:%d ", ticks);
    //         // DBprintf("ij:%d %d ", i, j);
    //         // DBprintf("e1,e2:%d, %d ", encoder_calibration_buff[(i + 1) % self->param_encoder_cal_count_max], encoder_calibration_buff[i % self->param_encoder_cal_count_max]);
    //         // DBprintf("\r\n");
    //     }
    // }

    // DBprintf("flash write start ---------------------\r\n");

    // int32 lookupAngle2 = 0;
    // uint32 n = 0;
    // // self->mcu->FlashUnlock();
    // // self->mcu->FlashEraseOnePage32KBytes();

    // for (int32_t i = iStart; i < (iStart + self->param_encoder_cal_count_max + 1); i++) // 以步进电机整步为基准（小于0.08度）开始对编码器进行校正插值并将校正后的值存入FLASH
    // {
    //     ticks = encoder_calibration_buff[(i + 1) % self->param_encoder_cal_count_max] - encoder_calibration_buff[i % self->param_encoder_cal_count_max];
    //     if (ticks < -(self->encoder->param_encoder_max_pulse * 0.9))
    //     {
    //         ticks += self->encoder->param_encoder_max_pulse;
    //     }
    //     if (i == iStart)
    //     {
    //         for (int32_t j = jStart; j < ticks; j++)
    //         {
    //             // lookupAngle = ((self->encoder->param_encoder_max_pulse / 2) * i + (self->encoder->param_encoder_max_pulse / 2) * j / ticks) % (self->encoder->param_encoder_max_pulse * 100) / 100;
    //             lookupAngle2 = (int32)(fmodf((float32)i + (float32)j / ticks, (float32)self->param_encoder_cal_count_max) * (float32)self->encoder->param_encoder_max_pulse / self->param_encoder_cal_count_max);
    //             // self->mcu->FlashWriteHalfWord(n, (uint16_t)lookupAngle2);
    //             // self->mcu->FlashWriteHalfWord(n, (uint16_t)n);
    //             // address += 2;
    //             // DBprintf("%d\t %d\t %d \r\n", n, lookupAngle, lookupAngle2);
    //             // DBprintf("ij:%d %d ", i, j);
    //             // DBprintf("e1,e2:%d, %d ", encoder_calibration_buff[(i + 1) % self->param_encoder_cal_count_max], encoder_calibration_buff[i % self->param_encoder_cal_count_max]);
    //             // DBprintf("encoder_raw:%d flash_addr:%d encoder_cal:%d ", encoder_calibration_buff[i % self->param_encoder_cal_count_max]+j, n, lookupAngle2);

    //             // DBprintf("%ld %lu %ld ", encoder_calibration_buff[i % self->param_encoder_cal_count_max] + j, n, lookupAngle2);
    //             DBprintf("%lu %ld ", n, lookupAngle2);

    //             // DBprintf("%d %d ", n, (encoder_calibration_buff[i % self->param_encoder_cal_count_max]+j)%self->encoder->param_encoder_max_pulse);
    //             // DBprintf("%d\r\n", n);
    //             DBprintf("\r\n");

    //             self->mcu->DelayMs(self->mcu, 1);
    //             n++;
    //         }
    //     }
    //     else if (i == (iStart + self->param_encoder_cal_count_max))
    //     {
    //         for (int32_t j = 0; j < jStart; j++)
    //         {
    //             // lookupAngle = (((self->encoder->param_encoder_max_pulse / 2) * i + (self->encoder->param_encoder_max_pulse / 2) * j / ticks) % (self->encoder->param_encoder_max_pulse * 100)) / 100;
    //             lookupAngle2 = (int32)(fmodf((float32)i + (float32)j / ticks, (float32)self->param_encoder_cal_count_max) * (float32)self->encoder->param_encoder_max_pulse / self->param_encoder_cal_count_max);
    //             // self->mcu->FlashWriteHalfWord(n, (uint16_t)lookupAngle2);
    //             // self->mcu->FlashWriteHalfWord(n, (uint16_t)n);
    //             // address += 2;
    //             // DBprintf("%d\t %d\t %d \r\n", n, lookupAngle, lookupAngle2);
    //             // DBprintf("ij:%d %d ", i, j);
    //             // DBprintf("e1,e2:%d, %d ", encoder_calibration_buff[(i + 1) % self->param_encoder_cal_count_max], encoder_calibration_buff[i % self->param_encoder_cal_count_max]);
    //             // DBprintf("encoder_raw:%d flash_addr:%d encoder_cal:%d ", encoder_calibration_buff[i % self->param_encoder_cal_count_max]+j, n, lookupAngle2);

    //             // DBprintf("%ld %lu %ld ", encoder_calibration_buff[i % self->param_encoder_cal_count_max] + j, n, lookupAngle2);
    //             DBprintf("%lu %ld ", n, lookupAngle2);

    //             // DBprintf("%d %d ", n, (encoder_calibration_buff[i % self->param_encoder_cal_count_max]+j)%self->encoder->param_encoder_max_pulse);
    //             // DBprintf("%d\r\n", n);
    //             DBprintf("\r\n");
    //             self->mcu->DelayMs(self->mcu, 1);
    //             n++;
    //         }
    //     }
    //     else
    //     {
    //         // this is the general case
    //         for (int32_t j = 0; j < ticks; j++)
    //         {
    //             // lookupAngle = (((self->encoder->param_encoder_max_pulse / 2) * i + (self->encoder->param_encoder_max_pulse / 2) * j / ticks) % (self->encoder->param_encoder_max_pulse * 100)) / 100;
    //             lookupAngle2 = (int32)(fmodf((float32)i + (float32)j / ticks, (float32)self->param_encoder_cal_count_max) * (float32)self->encoder->param_encoder_max_pulse / self->param_encoder_cal_count_max);
    //             // self->mcu->FlashWriteHalfWord(n, (uint16_t)lookupAngle2);
    //             // self->mcu->FlashWriteHalfWord(n, (uint16_t)n);
    //             // address += 2;
    //             // DBprintf("%d\t %d\t %d \r\n", n, lookupAngle, lookupAngle2);
    //             // DBprintf("ij:%d %d ", i, j);
    //             // DBprintf("e1,e2:%d, %d ", encoder_calibration_buff[(i + 1) % self->param_encoder_cal_count_max], encoder_calibration_buff[i % self->param_encoder_cal_count_max]);
    //             // DBprintf("encoder_raw:%d flash_addr:%d encoder_cal:%d ", encoder_calibration_buff[i % self->param_encoder_cal_count_max]+j, n, lookupAngle2);

    //             // DBprintf("%ld %lu %ld ", encoder_calibration_buff[i % self->param_encoder_cal_count_max] + j, n, lookupAngle2);
    //             DBprintf("%lu %ld ", n, lookupAngle2);

    //             // DBprintf("%d %d ", n, (encoder_calibration_buff[i % self->param_encoder_cal_count_max]+j)%self->encoder->param_encoder_max_pulse);
    //             // DBprintf("%d\r\n", n);
    //             DBprintf("\r\n");
    //             self->mcu->DelayMs(self->mcu, 1);
    //             n++;
    //         }
    //     }
    // }

    // // self->mcu->FlashLock();

    // /* test flash data */
    // DBprintf("test flash data start-------------------------\r\n");
    // // volatile uint16 read_data = 0;
    // // volatile uint16 *read_data_addr = 0;

    // // const uint32 loopmax = (32UL * 1024UL)/2;
    // // for (ppi = 0; ppi < loopmax; ppi++)
    // // {
    // //     read_data_addr = (volatile uint16 *)(0x08020000 + ppi*2UL);

    // //     DBprintf("%d\t %#x\r\n", ppi, *read_data_addr);
    // //     self->mcu->DelayMs(self->mcu, 10);
    // // }

    // // read_data = *(volatile uint16 *)((uint32_t)0x08020000 + 0);
    // // DBprintf("%#x\r\n", read_data);
    // // DBprintf("%d\r\n", read_data);

    // // read_data = *(volatile uint16 *)((uint32_t)0x08020000 + 2*(8192-1));
    // // DBprintf("%#x\r\n", read_data);
    // // DBprintf("%d\r\n", read_data);

    // // read_data = *(volatile uint16 *)((uint32_t)0x08020000 + 2*(16384-1));
    // // DBprintf("%#x\r\n", read_data);
    // // DBprintf("%d\r\n", read_data);

    // DBprintf("end-------------------------\r\n");

    // free(encoder_calibration_buff);
    return 1;
}
//
uint8 StepperMotorController_EncoderCalibration(struct StepperMotorController *self, uint8_t debug_output)
{
    // int32 *encoder_calibration_buff;
    // int32 encoder_sum = 0;
    // int32 encoder_read = 0;

    // encoder_calibration_buff = malloc(self->param_encoder_cal_count_max * sizeof(int32));

    // if (debug_output != 0)
    // {
    //     DBprintf("Encoder calibration start\r\n");
    //     DBprintf("Encoder calibration running......\r\n");
    // }

    // /* set pwm to 0 */
    // self->driver_board->param_pwm_output_en = 1;
    // self->controller->foc.ud = 0;
    // self->controller->foc.uq = 0;
    // self->controller->foc.theta = 0;
    // self->UdUqOutput(self);
    // self->mcu->DelayMs(self->mcu, 500);

    // /* init encodedr */
    // int32 encoder_direction_backup = self->encoder->param_direction;
    // self->encoder->param_direction = 1;
    // self->encoder->GetEncoderCalibrated(self->encoder);
    // self->encoder->GetEncoderSingleturn(self->encoder);
    // self->encoder->GetEncoderSingleturn(self->encoder);

    // /* init */
    // self->controller->foc.ud = self->param_ud_cal_set;
    // self->controller->foc.uq = 0;
    // self->controller->foc.theta = 0;
    // self->UdUqOutput(self);
    // self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_step * 2);

    // if (debug_output != 0)
    // {
    //     DBprintf("-------forward start\r\n");
    // }

    // /* forward rotation */
    // for (int16 i = 0; i < self->param_encoder_cal_count_max; i++)
    // {
    //     self->controller->foc.ud = self->param_ud_cal_set;
    //     self->controller->foc.uq = 0;
    //     self->controller->foc.theta = fmodf(i * (BM_2PI * (float32)self->stepper_motor->param_pole_pair / (float32)self->param_encoder_cal_count_max), BM_2PI);
    //     self->UdUqOutput(self);

    //     self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_step);
    //     encoder_sum = 0;
    //     for (int16 j = 0; j < self->param_encoder_read_count; j++)
    //     {
    //         encoder_read = self->encoder->GetEncoder(self->encoder);
    //         encoder_sum += encoder_read;
    //         self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_read);
    //     }
    //     encoder_sum = encoder_sum / (int16)(self->param_encoder_read_count);

    //     if (debug_output != 0)
    //     {
    //         DBprintf("%ld\r\n", encoder_sum);
    //     }

    //     encoder_calibration_buff[i] = encoder_sum;
    // }

    // if (debug_output != 0)
    // {
    //     DBprintf("-------backward start\r\n");
    // }

    // /* backward rotation */
    // for (int16 i = self->param_encoder_cal_count_max - 1; i >= 0; i--)
    // {
    //     self->controller->foc.ud = self->param_ud_cal_set;
    //     self->controller->foc.uq = 0;
    //     self->controller->foc.theta = fmodf(i * (BM_2PI * (float32)self->stepper_motor->param_pole_pair / (float32)self->param_encoder_cal_count_max), BM_2PI);
    //     self->UdUqOutput(self);

    //     self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_step);
    //     encoder_sum = 0;
    //     for (int16 j = 0; j < self->param_encoder_read_count; j++)
    //     {
    //         encoder_read = self->encoder->GetEncoder(self->encoder);
    //         encoder_sum += encoder_read;
    //         self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_read);
    //     }
    //     encoder_sum = encoder_sum / (int16)(self->param_encoder_read_count);

    //     if (debug_output != 0)
    //     {
    //         DBprintf("%ld\r\n", encoder_sum);
    //     }

    //     encoder_calibration_buff[i] = (encoder_calibration_buff[i] + encoder_sum) / 2;

    //     if (encoder_calibration_buff[i] > self->encoder->param_encoder_max_pulse)
    //     {
    //         encoder_calibration_buff[i] -= self->encoder->param_encoder_max_pulse;
    //     }
    //     else if (encoder_calibration_buff[i] < 0)
    //     {
    //         while (encoder_calibration_buff[i] < 0)
    //         {
    //             encoder_calibration_buff[i] += self->encoder->param_encoder_max_pulse;
    //         }
    //     }
    // }

    // self->controller->foc.ud = 0;
    // self->controller->foc.uq = 0;
    // self->controller->foc.theta = 0;
    // self->driver_board->param_pwm_output_en = 0;
    // self->UdUqOutput(self);
    // self->mcu->DelayMs(self->mcu, 500);

    // /* average */
    // if (debug_output != 0)
    // {
    //     for (int16 i = 0; i < self->param_encoder_cal_count_max; i++)
    //     {
    //         DBprintf("%d\t %ld \r\n", i, encoder_calibration_buff[i]);
    //         self->mcu->DelayMs(self->mcu, 1);
    //     }

    //     DBprintf("end-------------------------\r\n");
    // }

    // self->encoder->param_direction = encoder_direction_backup;

    // free(encoder_calibration_buff);
    return 1;
}

void StepperMotorController_UdUqOutput(struct StepperMotorController *self)
{
    /* ipark */
    self->controller->foc.IPark(self->controller->foc.ud,
                                self->controller->foc.uq,
                                self->controller->foc.theta,
                                &self->controller->foc.ualpha,
                                &self->controller->foc.ubeta);

    /* chopper */
    self->chopper.in_Ualpha = self->controller->foc.ualpha;
    self->chopper.in_Ubeta = self->controller->foc.ubeta;
    self->chopper.Output(&self->chopper);

    /* set pwm */
    self->driver_board->in_pwm_duty_A1 = self->chopper.out_Duty_A1;
    self->driver_board->in_pwm_duty_A2 = self->chopper.out_Duty_A2;
    self->driver_board->in_pwm_duty_B1 = self->chopper.out_Duty_B1;
    self->driver_board->in_pwm_duty_B2 = self->chopper.out_Duty_B2;
    self->driver_board->UpdatePWM(self->driver_board);
}

void StepperMotorController_DriverEnableSolution(struct StepperMotorController *self)
{
#ifndef HARDWARE_DISABLE
    /* en processing */
    self->step_controller.en_pin_last = self->step_controller.en_pin;
    // self->step_controller.en_pin = self->mcu->GpioRead(self->driver_board->param_pin_EN.pin);
    if ((self->step_controller.en_pin_last == 0) && (self->step_controller.en_pin == 1))
    {
        self->DriverEnableControl(self, 0);
    }
    else if ((self->step_controller.en_pin_last == 1) && (self->step_controller.en_pin == 0))
    {
        if (self->protection.err_code == 0)
        {
            self->DriverEnableControl(self, 1);
        }
    }
    else if (self->driver_board->param_pwm_output_en == 0)
    {
        self->step_controller.SetInitStepFromRad(&self->step_controller, self->encoder_fdb * self->encoder_2_pos_rad);
        // self->in_pos_cmd = self->step_controller.step * self->step_controller.step2rad * self->param_motion_dir;
        self->in_pos_cmd = self->param_motion_dir > 0 ? (self->step_controller.step * self->step_controller.step2rad) : (BM_2PI - self->step_controller.step * self->step_controller.step2rad);
    }

#endif
}

void StepperMotorController_StallDetect(struct StepperMotorController *self)
{
    if (self->param_stall_mode == StallMode_Cur)
    {
        if (fabsf(self->controller->iq_loop_pid.in_ref) > self->param_stall_cur_A)
        {
            self->stall_is_trigger = 1;
        }
        else
        {
            self->stall_is_trigger = 0;
        }
    }
    else if (self->param_stall_mode == StallMode_Pos)
    {
        if (fabsf(self->controller->pos_loop_pid.err) > self->param_stall_pos_err_rad)
        {
            self->stall_is_trigger = 1;
        }
        else
        {
            self->stall_is_trigger = 0;
        }
    }

    if (self->param_emergency_pin_mode == Emergency_Pin_Mode_Stall)
    {
        if (self->stall_is_trigger != 0)
        {
            // self->mcu->GpioSet(self->driver_board->param_pin_EMERGENCY.pin);
        }
        else
        {
            // self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);
        }
    }
}

void StepperMotorController_ProtectionCheckErr(struct StepperMotorController *self, uint16 err_num)
{
    int32 tmp;
    if (self->protection.param_en != 0)
    {
        switch (err_num)
        {
        case ERR_NUM_ENCODER_MUTATION:
            tmp = abs_bm(self->encoder_fdb - self->encoder_fdb_last);
            if (tmp < self->protection.param_encoder_mutation_upper_encoder_pulse &&
                tmp > self->protection.param_encoder_mutation_lower_encoder_pulse)
            {
                // DBprintf("%d %d\r\n", self->encoder_fdb, self->encoder_fdb_last);
                self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_ENCODER_MUTATION);
                self->DriverEnableControl(self, 0);
            }
            break;
        case ERR_NUM_ENCODER_READ_ERR:
            if (self->encoder->varify_pass == 0)
            {
                self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_ENCODER_READ_ERR);
            }
            break;
        case ERR_NUM_SOFTWARE_PEAK_CURRENT_OVER_LIMIT:
            if (fabsf(self->controller->in_ialpha_fdb) > self->protection.param_peak_overcurrent_A ||
                fabsf(self->controller->in_ibeta_fdb) > self->protection.param_peak_overcurrent_A)
            {
                self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_SOFTWARE_PEAK_CURRENT_OVER_LIMIT);
                self->DriverEnableControl(self, 0);
            }
            break;
        case ERR_NUM_SOFTWARE_CONTINUE_CURRENT_OVER_LIMIT:
            if (fabsf(self->controller->in_ialpha_fdb) > self->protection.param_continuous_overcurrent_A ||
                fabsf(self->controller->in_ibeta_fdb) > self->protection.param_continuous_overcurrent_A)
            {
                self->protection.continuous_overcurrent_count++;
                if (self->protection.continuous_overcurrent_count > self->protection.continuous_overcurrent_count_max)
                {
                    self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_SOFTWARE_CONTINUE_CURRENT_OVER_LIMIT);
                    self->DriverEnableControl(self, 0);
                }
            }
            else
            {
                self->protection.continuous_overcurrent_count = 0;
            }
            break;
        case ERR_NUM_HARDWARE_CURRENT_OVER_LIMIT:
            // self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_HARDWARE_CURRENT_OVER_LIMIT);
            break;
        case ERR_NUM_SPEED_FDB_CONTINUE_OVER_LIMIT:
            if (fabsf(self->controller->spd_fdb_ctrl) > self->protection.param_speed_over_limit_rad_s)
            {
                self->protection.continuous_overspeed_count++;
                if (self->protection.continuous_overspeed_count > self->protection.continuous_overspeed_count_max)
                {
                    self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_SPEED_FDB_CONTINUE_OVER_LIMIT);
                    self->DriverEnableControl(self, 0);
                }
            }
            else
            {
                self->protection.continuous_overspeed_count = 0;
            }
            break;
        case ERR_NUM_POS_FDB_OVER_LIMIT:
            if (self->pos_fdb_encoder > self->protection.param_pos_over_limit_max_rad ||
                self->pos_fdb_encoder < self->protection.param_pos_over_limit_min_rad)
            {
                self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_POS_FDB_OVER_LIMIT);
            }
            break;
        case ERR_NUM_POS_CMD_MUTATION:
            if (((float)abs_bm(self->step_controller.step_increment) / self->step_controller.steps_per_turn) > self->protection.param_pos_cmd_mutation_turns)
            {
                self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_POS_CMD_MUTATION);
                self->DriverEnableControl(self, 0);
            }
            break;
        case ERR_NUM_EXCESSIVE_POS_TRACKING_ERROR:
            if (self->controller->param_en_all_control_switch != 0 &&
                abs_bm(self->controller->pos_loop_pid.err) > self->protection.param_pos_tracking_error_maximum_rad)
            {
                self->protection.continuous_pos_tracking_error_count++;
                if (self->protection.continuous_pos_tracking_error_count > self->protection.continuous_pos_tracking_error_count_max)
                {
                    self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_EXCESSIVE_POS_TRACKING_ERROR);
                    self->DriverEnableControl(self, 0);
                }
            }
            else
            {
                self->protection.continuous_pos_tracking_error_count = 0;
            }

            // if (self->controller->param_en_all_control_switch != 0 &&
            //     abs_bm(self->controller->pos_loop_pid.err) > self->protection.param_pos_tracking_error_maximum_rad)
            // {
            //     self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_EXCESSIVE_POS_TRACKING_ERROR);
            //     self->DriverEnableControl(self, 0);
            // }
            break;
        case ERR_NUM_MOTOR_CHECK_ValueAbnormal ... ERR_NUM_MOTOR_CHECK_OtherErr:
            switch (self->motor_phase_check.failure)
            {
            case MotorPhaseCheck_Failure_UnStable:
                self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_MOTOR_CHECK_UnStable);
                break;
            case MotorPhaseCheck_Failure_ValueAbnormal:
                self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_MOTOR_CHECK_ValueAbnormal);
                break;
            case MotorPhaseCheck_Failure_MotorPhaseNotConnected:
                self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_MOTOR_CHECK_MotorPhaseNotConnected);
                break;
            case MotorPhaseCheck_Failure_MotorPhaseResistanceDifferent:
                self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_MOTOR_CHECK_MotorPhaseResistanceDifferent);
                break;
            case MotorPhaseCheck_Failure_OtherErr:
                self->protection.err_code_raw |= PROTECTION_CODE(ERR_NUM_MOTOR_CHECK_OtherErr);
                break;
            }
            self->motor_phase_check.failure = MotorPhaseCheck_Failure_NoFailure;
            break;
        default:
            break;
        }
        // self->protection.err_code_last = self->protection.err_code;
        self->protection.err_code = self->protection.err_code_raw & self->protection.param_err_code_mask;
    }
}

void StepperMotorController_ProtectionCheckWarning(struct StepperMotorController *self, uint16 warning_num)
{
    if (self->protection.param_en != 0)
    {
        switch (warning_num)
        {
        case WARN_NUM_COMMUNICATION_ERR:

            break;
        case WARN_NUM_MOTOR_POWER_SUPPLY_TOO_LOW:
            if (self->driver_board->board_detect_power_supply_V < self->protection.param_power_voltage_min)
            {
                self->protection.warning_code_raw |= PROTECTION_CODE(WARN_NUM_MOTOR_POWER_SUPPLY_TOO_LOW);
            }
            break;
        case WARN_NUM_MCU_OVER_HEATING:
            if (self->driver_board->board_detect_temperature_C > self->protection.param_mcu_temp_max)
            {
                self->protection.warning_code_raw |= PROTECTION_CODE(WARN_NUM_MCU_OVER_HEATING);
            }
            break;
        default:
            break;
        }
        // self->protection.warning_code_last = self->protection.warning_code;
        self->protection.warning_code = self->protection.warning_code_raw & self->protection.param_warning_code_mask;
    }
}

void StepperMotorController_ProtectionDetection(struct StepperMotorController *self)
{
    /* Check err_code */
    self->protection.err_code_last = self->protection.err_code;
    self->protection.warning_code_last = self->protection.warning_code;

    self->ProtectionCheckErr(self, ERR_NUM_ENCODER_MUTATION);
    // self->ProtectionCheckErr(self, ERR_NUM_ENCODER_READ_ERR);
    self->ProtectionCheckErr(self, ERR_NUM_SOFTWARE_PEAK_CURRENT_OVER_LIMIT);
    self->ProtectionCheckErr(self, ERR_NUM_SOFTWARE_CONTINUE_CURRENT_OVER_LIMIT);
    // self->ProtectionCheckErr(self, ERR_NUM_HARDWARE_CURRENT_OVER_LIMIT);
    self->ProtectionCheckErr(self, ERR_NUM_SPEED_FDB_CONTINUE_OVER_LIMIT);
    // self->ProtectionCheckErr(self, ERR_NUM_POS_FDB_OVER_LIMIT);
    self->ProtectionCheckErr(self, ERR_NUM_POS_CMD_MUTATION);
    self->ProtectionCheckErr(self, ERR_NUM_EXCESSIVE_POS_TRACKING_ERROR);

    self->ProtectionCheckErr(self, ERR_NUM_MOTOR_CHECK_ValueAbnormal);

    /* Check warnning_code */
    // self->ProtectionCheckWarning(self, WARN_NUM_COMMUNICATION_ERR);
    self->ProtectionCheckWarning(self, WARN_NUM_MOTOR_POWER_SUPPLY_TOO_LOW);
    self->ProtectionCheckWarning(self, WARN_NUM_MCU_OVER_HEATING);

    /* Save protection code in flash */
    char write_tmp[10] = {0};
    if (self->protection.err_code_last != self->protection.err_code)
    {
        // sprintf(write_tmp, "%lu", self->protection.err_code);
        // self->flash_manager_param->WriteVariableIntoFlash(self->flash_manager_param, "protect_err_code_backup", write_tmp);
        // self->protection.err_code_backup = self->protection.err_code;

        self->protection.err_code_backup = self->protection.err_code;
        // self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "protect_err_code_backup");
    }

    if (self->protection.warning_code_last != self->protection.warning_code)
    {
        // sprintf(write_tmp, "%lu", self->protection.warning_code);
        // self->flash_manager_param->WriteVariableIntoFlash(self->flash_manager_param, "protect_warning_code_backup", write_tmp);
        // self->protection.warning_code_backup = self->protection.warning_code;

        self->protection.warning_code_backup = self->protection.warning_code;
        // self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "protect_warning_code_backup");
    }

    /* Change the level of emergency pin when protection is triggered */
    if (self->param_emergency_pin_mode == Emergency_Pin_Mode_ProtectionSignal)
    {
        // if (self->protection.err_code != 0 || self->protection.warning_code != 0)
        if (self->protection.err_code_last != self->protection.err_code ||
            self->protection.warning_code_last != self->protection.warning_code)
        {
            // self->mcu->GpioSet(self->driver_board->param_pin_EMERGENCY.pin);
        }
        else
        {
            // self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);
        }
    }

    self->protection.ReportErr(&self->protection);
    self->protection.ReportWarning(&self->protection);
}

void StepperMotorController_KeyScan(struct StepperMotorController *self)
{
    static uint32 key_count;
    uint32 key_count_max = (uint32)(0.010 / self->controller->param_pos_control_period_s);
    static uint16 key_lock = 0;
    // if (self->driver_board->KeyRead(self->driver_board, self->driver_board->param_pin_K1_out.pin))
    if (0)
    {
        key_count++;
        if (key_count > key_count_max)
        {
            key_count = 1;
            if (key_lock == 0)
            {
                key_lock = 1;
                if (self->driver_board->param_pwm_output_en == FALSE)
                {
                    self->driver_board->param_pwm_output_en = TRUE;
                    self->controller->param_en_all_control_switch = 1;
                    self->controller->leso.param_en = 1;
                }
                else
                {
                    self->driver_board->param_pwm_output_en = FALSE;
                    self->controller->param_en_all_control_switch = 0;
                    self->controller->leso.param_en = 0;
                }
            }
        }
    }
    else
    {
        key_lock = 0;
        key_count = 0;
    }
}

uint8 StepperMotorController_ElecZeroOffsetCalibration(struct StepperMotorController *self, uint8_t debug_output, uint8_t output_format)
{
    int32 *encoder_calibration_buff;
    int32 encoder_sum = 0;
    int32 encoder_read = 0;

    /* close protection */
    uint8 protect_tmp;
    protect_tmp = self->protection.param_en;
    self->protection.param_en = 0;

    encoder_calibration_buff = malloc(self->param_encoder_cal_count_max * sizeof(int32));
    if (debug_output != 0)
    {
        DBprintf("ElecZeroOffset calibration start\r\n");
        DBprintf("ElecZeroOffset calibration running......\r\n");
    }

    self->driver_board->param_pwm_output_en = 1;
    self->controller->foc.ud = 0;
    self->controller->foc.uq = 0;
    self->controller->foc.theta = 0;
    self->UdUqOutput(self);
    self->mcu->DelayMs(self->mcu, 500);

    /* init */
    self->controller->foc.ud = self->param_ud_cal_set;
    self->controller->foc.uq = 0;
    self->controller->foc.theta = 0;
    self->UdUqOutput(self);
    self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_step * 2);

    if (debug_output != 0)
    {
        DBprintf("-------forward start\r\n");
    }

    /* forward rotation */
    for (int16 i = 0; i < self->param_encoder_cal_count_max; i++)
    {
        // self->encoder->GetEncoder(self->encoder);
        // encoder_read_last = self->encoder->encoder_pulse_single_turn;

        self->controller->foc.ud = self->param_ud_cal_set;
        self->controller->foc.uq = 0;
        self->controller->foc.theta = self->param_calibration_dir * fmodf(i * (BM_2PI * (float32)self->stepper_motor->param_pole_pair / (float32)self->param_encoder_cal_count_max), BM_2PI);
        self->UdUqOutput(self);

        self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_step);
        // self->encoder->GetEncoder(self->encoder);
        encoder_sum = 0;
        for (int16 j = 0; j < self->param_encoder_read_count; j++)
        {
            // self->encoder->GetEncoder(self->encoder);
            // encoder_read = self->encoder->encoder_pulse_single_turn;
            encoder_read = self->encoder->GetEncoder(self->encoder);
            // encoder_sum += self->encoder->GetEncoder(self->encoder) % self->encoder->param_encoder_max_pulse;
            // encoder_sum += self->encoder->GetEncoder(self->encoder);
            // if(encoder_read - encoder_read_last < -(self->encoder->param_encoder_max_pulse / 2))
            // {
            //     encoder_read += self->encoder->param_encoder_max_pulse;
            // }
            // else if(encoder_read - encoder_read_last > (self->encoder->param_encoder_max_pulse / 2))
            // {
            //     encoder_read -= self->encoder->param_encoder_max_pulse;
            // }
            encoder_sum += encoder_read;
            self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_read);
        }
        encoder_sum = encoder_sum / (int16)(self->param_encoder_read_count);

        // if(encoder_sum > self->encoder->param_encoder_max_pulse)
        // {
        //     encoder_sum -= self->encoder->param_encoder_max_pulse;
        // }
        // else if(encoder_sum < 0)
        // {
        //     encoder_sum += self->encoder->param_encoder_max_pulse;
        // }
        // DBprintf("%ld\r\n", encoder_sum);
        if (debug_output != 0)
        {
            DBprintf("%ld %ld\r\n", (int32)(self->controller->foc.theta * 1000), encoder_sum);
        }
        encoder_calibration_buff[i] = encoder_sum;
        // DBprintf("%d %d\r\n", i, encoder_calibration_buff[i]);
    }

    if (debug_output != 0)
    {
        DBprintf("-------backward start\r\n");
    }

    /* backward rotation */
    for (int16 i = self->param_encoder_cal_count_max - 1; i >= 0; i--)
    {

        // self->encoder->GetEncoder(self->encoder);
        // encoder_read_last = self->encoder->encoder_pulse_single_turn;

        self->controller->foc.ud = self->param_ud_cal_set;
        self->controller->foc.uq = 0;
        self->controller->foc.theta = self->param_calibration_dir * fmodf(i * (BM_2PI * (float32)self->stepper_motor->param_pole_pair / (float32)self->param_encoder_cal_count_max), BM_2PI);
        self->UdUqOutput(self);

        self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_step);
        // self->encoder->GetEncoder(self->encoder);
        encoder_sum = 0;
        for (int16 j = 0; j < self->param_encoder_read_count; j++)
        {
            // self->encoder->GetEncoder(self->encoder);
            // encoder_read = self->encoder->encoder_pulse_single_turn;
            encoder_read = self->encoder->GetEncoder(self->encoder);
            // encoder_sum += self->encoder->GetEncoder(self->encoder) % self->encoder->param_encoder_max_pulse;
            // encoder_sum += self->encoder->GetEncoder(self->encoder);
            // if(encoder_read - encoder_read_last < -(self->encoder->param_encoder_max_pulse / 2))
            // {
            //     encoder_read += self->encoder->param_encoder_max_pulse;
            // }
            // else if(encoder_read - encoder_read_last > (self->encoder->param_encoder_max_pulse / 2))
            // {
            //     encoder_read -= self->encoder->param_encoder_max_pulse;
            // }
            encoder_sum += encoder_read;
            self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_read);
        }
        // encoder_calibration_buff[self->param_encoder_cal_count_max + i] = encoder_sum / (int16)(self->param_encoder_read_count);
        encoder_sum = encoder_sum / (int16)(self->param_encoder_read_count);
        // if(encoder_sum > self->encoder->param_encoder_max_pulse)
        // {
        //     encoder_sum -= self->encoder->param_encoder_max_pulse;
        // }
        // else if(encoder_sum < 0)
        // {
        //     encoder_sum += self->encoder->param_encoder_max_pulse;
        // }
        // DBprintf("%ld\r\n", encoder_sum);

        if (debug_output != 0)
        {
            DBprintf("%ld %ld\r\n", (int32)(self->controller->foc.theta * 1000), encoder_sum);
        }

        // if(fabs((encoder_calibration_buff[i] - encoder_sum)) > self->encoder->param_encoder_max_pulse*0.5)
        // {
        //     encoder_calibration_buff[i] = (encoder_calibration_buff[i] + encoder_sum + ) / 2;
        // }
        // else
        // {
        //     encoder_calibration_buff[i] = (encoder_calibration_buff[i] + encoder_sum) / 2;
        // }

        // if (abs_bm(encoder_calibration_buff[i] - encoder_sum) > self->encoder->param_encoder_max_pulse>>1)
        // {
        //     encoder_sum += self->encoder->param_encoder_max_pulse;
        // }

        encoder_calibration_buff[i] = (encoder_calibration_buff[i] + encoder_sum) / 2;

        if (encoder_calibration_buff[i] > self->encoder->param_encoder_max_pulse)
        {
            encoder_calibration_buff[i] -= self->encoder->param_encoder_max_pulse;
        }
        else if (encoder_calibration_buff[i] < 0)
        {
            while (encoder_calibration_buff[i] < 0)
            {
                encoder_calibration_buff[i] += self->encoder->param_encoder_max_pulse;
            }
        }
        // DBprintf("%d %d\r\n", i, encoder_calibration_buff[i]);
    }

    self->controller->foc.ud = 0;
    self->controller->foc.uq = 0;
    self->controller->foc.theta = 0;
    self->driver_board->param_pwm_output_en = 0;
    self->UdUqOutput(self);
    self->mcu->DelayMs(self->mcu, 500);

    /* close protection */
    self->protection.param_en = protect_tmp;

    if (debug_output != 0)
    {
        DBprintf("average ---------------------\r\n");
    }

    /* average */
    for (int16 i = 0; i < self->param_encoder_cal_count_max; i++)
    {
        if (debug_output != 0)
        {
            DBprintf("%d\t %ld \r\n", i, encoder_calibration_buff[i]);
        }
        self->mcu->DelayMs(self->mcu, 1);
    }
    if (debug_output != 0)
    {
        DBprintf("pos & zero offset ---------------------\r\n");
    }
    /* sum */
    float32 elec_angle_offset_max = 0;
    float32 elec_angle_offset_min = 999999;
    float32 elec_angle_offset_average = 0;
    float32 elec_angle_offset_err = 0;
    for (int16 i = 0; i < self->param_encoder_cal_count_max; i += 4)
    {
        float32 elec_period = 0;
        static float32 elec_zero_offset_last = 0;
        float32 elec_zero_offset = 0;
        elec_period = (float32)self->encoder->param_encoder_max_pulse / (float32)self->stepper_motor->param_pole_pair;

        elec_zero_offset = fmodf(encoder_calibration_buff[i], elec_period);
        if (i == 0)
        {
            elec_zero_offset_last = elec_zero_offset;
        }

        // DBprintf("::%s %s %s\r\n", float2str(elec_zero_offset, 4), float2str(elec_zero_offset_last, 4), float2str(elec_zero_offset - elec_zero_offset_last, 4));

        /* 边界检测及修正 */
        if (elec_zero_offset - elec_zero_offset_last > elec_period * 0.5)
        {
            elec_zero_offset -= elec_period;
        }
        else if (elec_zero_offset - elec_zero_offset_last < -elec_period * 0.5)
        {
            elec_zero_offset += elec_period;
        }
        // DBprintf("==%s %s\r\n", float2str(elec_zero_offset, 4), float2str(elec_zero_offset_last, 4));
        elec_zero_offset_last = elec_zero_offset;

        /* 记录零位最大偏差 */
        if (elec_zero_offset > elec_angle_offset_max)
        {
            elec_angle_offset_max = elec_zero_offset;
        }
        if (elec_zero_offset < elec_angle_offset_min)
        {
            elec_angle_offset_min = elec_zero_offset;
        }
        elec_angle_offset_average += elec_zero_offset;
        if (debug_output != 0)
        {
            DBprintf("%ld\t %s\t \r\n", encoder_calibration_buff[i], float2str(fmodf(encoder_calibration_buff[i], elec_period), 2));
        }
        self->mcu->DelayMs(self->mcu, 1);
    }

    elec_angle_offset_average = elec_angle_offset_average * 4 / self->param_encoder_cal_count_max;
    elec_angle_offset_err = elec_angle_offset_max - elec_angle_offset_min;
    // DBprintf("elec_angle_offset_max: %ld.%ld\r\n", (int32)(elec_angle_offset_max), (int32)(elec_angle_offset_max * 100) % 100);
    // DBprintf("elec_angle_offset_min: %ld.%ld\r\n", (int32)(elec_angle_offset_min), (int32)(elec_angle_offset_min * 100) % 100);
    // DBprintf("elec_angle_offset_err: %ld.%ld\r\n", (int32)(elec_angle_offset_err), (int32)(elec_angle_offset_err * 100) % 100);
    // DBprintf("elec_angle_offset_average: %ld.%ld\r\n", (int32)(elec_angle_offset_average), (int32)(elec_angle_offset_average * 100) % 100);

    self->elec_offset_err_deg = (elec_angle_offset_err * 360.0F / ((float)self->encoder->param_encoder_max_pulse / self->stepper_motor->param_pole_pair));
    // self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "elec_offset_err_deg");

    /* cal */
    self->elec_angle_calibration_info.elec_angle_offset_max_deg = (elec_angle_offset_max * 360.0F / ((float)self->encoder->param_encoder_max_pulse / self->stepper_motor->param_pole_pair));
    self->elec_angle_calibration_info.elec_angle_offset_min_deg = (elec_angle_offset_min * 360.0F / ((float)self->encoder->param_encoder_max_pulse / self->stepper_motor->param_pole_pair));
    self->elec_angle_calibration_info.elec_angle_offset_err_deg = (elec_angle_offset_err * 360.0F / ((float)self->encoder->param_encoder_max_pulse / self->stepper_motor->param_pole_pair));
    self->elec_angle_calibration_info.elec_angle_offset_average_deg = (elec_angle_offset_average * 360.0F / ((float)self->encoder->param_encoder_max_pulse / self->stepper_motor->param_pole_pair));

    self->elec_angle_calibration_info.elec_angle_offset_max_pulse = elec_angle_offset_max;
    self->elec_angle_calibration_info.elec_angle_offset_min_pulse = elec_angle_offset_min;
    self->elec_angle_calibration_info.elec_angle_offset_err_pulse = elec_angle_offset_err;
    self->elec_angle_calibration_info.elec_angle_offset_average_pulse = elec_angle_offset_average;

    self->elec_angle_calibration_info.elec_angle_offset_err_pass = self->elec_angle_calibration_info.elec_angle_offset_err_deg < self->param_elec_angle_calibration_standar_deg;

    if (self->elec_angle_calibration_info.elec_angle_offset_err_pass)
    {
        /* write elec_angle_offset into flash */
        self->param_elec_offset = elec_angle_offset_average;
        // char write_tmp[10] = {0};
        // sprintf(write_tmp, "%s", float2str(elec_angle_offset_average, 4));
        // self->flash_manager_param->WriteVariableIntoFlash(self->flash_manager_param, "elec_offset", write_tmp);

        if (debug_output != 0)
        {
            DBprintf("Waiting......\r\n");
        }
        // self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "elec_offset");
        self->mcu->DelayMs(self->mcu, 100);
        self->flash_manager_param->SaveAllVariableIntoFlash(self->flash_manager_param);
    }

    if (output_format == ResultOutputFormat_Str)
    {
        DBprintf("elec_angle_offset_max: %s (deg)| %s (pulse)\r\n", float2str(self->elec_angle_calibration_info.elec_angle_offset_max_deg, 2), float2str(self->elec_angle_calibration_info.elec_angle_offset_max_pulse, 2));
        DBprintf("elec_angle_offset_min: %s (deg)| %s (pulse)\r\n", float2str(self->elec_angle_calibration_info.elec_angle_offset_min_deg, 2), float2str(self->elec_angle_calibration_info.elec_angle_offset_min_pulse, 2));
        DBprintf("elec_angle_offset_err: %s (deg)| %s (pulse)\r\n", float2str(self->elec_angle_calibration_info.elec_angle_offset_err_deg, 2), float2str(self->elec_angle_calibration_info.elec_angle_offset_err_pulse, 2));
        DBprintf("elec_angle_offset_average: %s (deg)| %s (pulse)\r\n", float2str(self->elec_angle_calibration_info.elec_angle_offset_average_deg, 2), float2str(self->elec_angle_calibration_info.elec_angle_offset_average_pulse, 2));

        DBprintf("\r\n");
        if (self->elec_angle_calibration_info.elec_angle_offset_err_pass)
        {
            DBprintf("Result: [pass]\r\n");
        }
        else
        {
            DBprintf("Result: [failed]\r\n");
        }

        // DBprintf("end-------------------------\r\n");
    }
    else if (output_format == ResultOutputFormat_Hex)
    {
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->elec_angle_calibration_info.elec_angle_offset_err_pass);

        data_conversion.f32 = self->elec_angle_calibration_info.elec_angle_offset_err_deg;
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 0) & 0xff);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 8) & 0xff);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 16) & 0xff);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 24) & 0xff);

        data_conversion.f32 = self->elec_angle_calibration_info.elec_angle_offset_average_pulse;
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 0) & 0xff);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 8) & 0xff);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 16) & 0xff);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 24) & 0xff);

        self->SendFormatedArray(self);
    }

    free(encoder_calibration_buff);

    return 1;
}

uint8 StepperMotorController_ReceiveDataAnalyse(struct StepperMotorController *self)
{
    if (self->communication_interface.receive_one_pack_ok)
    {
        self->communication_interface.receive_one_pack_ok = 0;

        if (self->communication_interface.receive_data_pack.pack_type >= VALID_NUM_OF_DATA_PACK_IN_TYPE_MOTION_CMD)
        {
            TERMINAL("Invalid datapack.\r\n");
            TERMINAL("Please enter 'help' to view the valid datapack type.\r\n");
        }
        else
        {
            self->CommandAnalyse(self);
            switch (self->communication_interface.receive_data_pack.pack_type)
            {
            case DATA_PACK_IN_TYPE_CMD:

                break;
            case DATA_PACK_IN_TYPE_MOTION_CMD:

                break;
            default:
                break;
            }
        }
        return 1;
    }
    return 0;
}

void StepperMotorController_CommandAnalyse(struct StepperMotorController *self)
{
    uint8 i;
    uint8 arg_num = 0;
    uint8 arg_str_i = 0;
    // uint8_t frame_process;
    // uint8_t addr, frame_length, status, function_code, parity_received;
    // uint8_t *data;
    // uint8_t data_len, parity_cal;

    char(*cmd_and_arg)[COMMAND_MAX_STRLEN] = self->communication_interface.cmd_and_arg;

    /* 清空命令buff */
    for (i = 0; i < COMMAND_MAX_NUM; i++)
    {
        cmd_and_arg[i][0] = 0;
    }

    if (self->communication_interface.receive_data_pack.databyte[0] == DataFrame_Head)
    {
        self->communication_interface.frame_process = DataFrameFormat_Addr;
        do
        {
            switch (self->communication_interface.frame_process)
            {
            // case DataFrameFormat_Head:
            //     if (self->communication_interface.receive_data_pack.databyte[0] == DataFrame_Head)
            //     {
            //         frame_process = DataFrameFormat_Addr;
            //     }
            //     else
            //     {
            //         frame_process = DataFrameFormat_End;
            //     }
            //     break;
            case DataFrameFormat_Addr:
                self->communication_interface.data_info.addr = self->communication_interface.receive_data_pack.databyte[1];
                if (self->communication_interface.data_info.addr != SlaveAddr_Broadcast &&
                    self->communication_interface.data_info.addr != self->rs485_addr)
                {
                    self->communication_interface.frame_process = DataFrameFormat_End;
                }
                else
                {
                    self->communication_interface.frame_process = DataFrameFormat_DataLength;
                }
                break;
            case DataFrameFormat_DataLength:
                self->communication_interface.data_info.frame_length = self->communication_interface.receive_data_pack.databyte[2];

                if (self->communication_interface.data_info.frame_length + 3 <= self->communication_interface.receive_data_pack.data_len)
                {
                    self->communication_interface.frame_process = DataFrameFormat_Status;
                }
                else
                {
                    self->communication_interface.frame_process = DataFrameFormat_End;
                }
                break;
            case DataFrameFormat_Status:
                self->communication_interface.data_info.status = self->communication_interface.receive_data_pack.databyte[3];
                self->communication_interface.frame_process = DataFrameFormat_FunctionCode;
                break;
            case DataFrameFormat_FunctionCode:
                self->communication_interface.data_info.function_code = self->communication_interface.receive_data_pack.databyte[4];
                self->communication_interface.frame_process = DataFrameFormat_Data;
                break;
            case DataFrameFormat_Data:
                self->communication_interface.data_info.data = &self->communication_interface.receive_data_pack.databyte[5];
                self->communication_interface.data_info.data_len = self->communication_interface.data_info.frame_length - 3;
                self->communication_interface.frame_process = DataFrameFormat_Parity;
                break;
            case DataFrameFormat_Parity:
                self->communication_interface.data_info.parity_received = self->communication_interface.receive_data_pack.databyte[self->communication_interface.data_info.frame_length + 2];
                self->communication_interface.data_info.parity_cal = crc8_cal(&self->communication_interface.receive_data_pack.databyte[2], self->communication_interface.data_info.frame_length);
                if (self->communication_interface.data_info.parity_received == self->communication_interface.data_info.parity_cal)
                {
                    self->communication_interface.is_hex_frame = TRUE;
                    /* 执行命令 */
                    if (self->communication_interface.data_info.function_code < self->cmd_nums)
                    {
                        if (!(self->cmd_table[self->communication_interface.data_info.function_code].broadcast_support == BroadcastSupport_No &&
                              self->communication_interface.data_info.addr == SlaveAddr_Broadcast))
                        {
                            self->cmd_table[self->communication_interface.data_info.function_code].func(self, (char(*)[COMMAND_MAX_STRLEN])(self->communication_interface.receive_data_pack.databyte));
                        }
                    }
                    self->communication_interface.is_hex_frame = FALSE;
                }
                self->communication_interface.frame_process = DataFrameFormat_End;
                break;
            default:
                self->communication_interface.frame_process = DataFrameFormat_End;
                break;
            }
        } while (self->communication_interface.frame_process != DataFrameFormat_End);
    }
    else if (self->communication_interface.param_char_cmd_support != 0)
    {
        // DBprintf("f\r\n");
        /* 从数据包中分离出参数 */
        for (i = 0; i < self->communication_interface.receive_data_pack.data_len; i++)
        {
            if (self->communication_interface.receive_data_pack.databyte[i] != ' ')
            {
                cmd_and_arg[arg_num][arg_str_i] = self->communication_interface.receive_data_pack.databyte[i];
                arg_str_i++;
            }
            else
            {
                cmd_and_arg[arg_num][arg_str_i] = '\0';
                arg_num++;
                arg_str_i = 0;
            }
        }
        cmd_and_arg[arg_num][arg_str_i] = '\0';

        /* 执行命令 */
        for (size_t i = 0; i < self->cmd_nums; i++)
        {
            if (strcmp((const char *)cmd_and_arg[0], self->cmd_table[i].name) == 0)
            {
                self->cmd_table[i].func(self, cmd_and_arg);
                break;
            }
        }
    }
}

void StepperMotorController_ClearInitFlag(struct StepperMotorController *self)
{
    self->init_flag.u16data = 0;
    self->init_done = 0;
}

void StepperMotorController_SystemReboot(struct StepperMotorController *self)
{
    self->mcu->DisableNvicInterrupt();
    self->driver_board->param_pwm_output_en = 0;
    self->mcu->SetAllPwm(self->mcu, 0, 0, 0, 0);
    // self->ClearInitFlag(self);
    // self->mcu->ResetSystem();

    self->mcu->EnableIWDG(1);
    self->mcu->DelayMs(self->mcu, 1); // delay > 1ms to trigger IWDG reset
}

void StepperMotorController_TimTask(struct StepperMotorController *self)
{

    // // self->mcu->GpioSet(self->driver_board->param_pin_EMERGENCY.pin);

    // // volatile static long long tick = -10500;
    // // tick++;
    // // volatile float in = tick * 0.4;

    // // DBprintf("%s %s %s\r\n", in,
    // //          float2str(fal(in, 0.75, 1), 4),
    // //          float2str(self->controller->spd_loop_pid.fal.Run(&self->controller->spd_loop_pid.fal, 4), float2str(in, 4)));

    // // self->mcu->GpioSet(self->driver_board->param_pin_EMERGENCY.pin);

    // // self->controller->fal_pos_err.Run(&self->controller->fal_pos_err, in);

    // // self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);

    // // // DBprintf("%s %s\r\n", float2str(0.004 * tick, 4), float2str(fal(0.004 * tick, 0.25, 5), 4));

    // // static uint32_t tick = 0;
    // // tick++;
    // // self->controller->td.in_v = sin_bm(BM_2PI*100.F*tick*3.2e-5F);
    // // self->controller->td.Run(&self->controller->td);
    // // DBprintf("%s %s %s %s\r\n",
    // // float2str(self->controller->td.in_v, 4),
    // // float2str(self->controller->td.out_x1, 4),
    // // float2str(self->controller->td.out_x2, 4),
    // // float2str(self->controller->td.out_g, 4));

    // // self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);

    // /* *****get fdb***** */
    // /* trigger adc sample */
    // // self->mcu->ADC_StartConversion();

    // /* read encoder 4.77us */
    // self->encoder_fdb_last = self->encoder_fdb;
    // self->encoder_fdb = self->encoder->GetEncoder(self->encoder);
    // // self->pos_fdb_abs = self->Encoder2PosAbs(self);
    // self->pos_fdb_encoder = self->Encoder2Pos(self);
    // self->spd_fdb_encoder = self->Encoder2Speed(self);

    // /* get elec angle 1.48us */
    // self->elec_angle_fdb = self->Encoder2ElecAngle(self);

    // /* get ia ib */
    // self->controller->in_ialpha_fdb = self->driver_board->GetIaFdb(self->driver_board);
    // self->controller->in_ibeta_fdb = (self->param_phase_order_invert == 0 ? 1.0F : -1.0F) * self->driver_board->GetIbFdb(self->driver_board);

    // /* step controller run*/
    // // self->DriverEnableSolution(self);
    // self->step_controller.Run(&self->step_controller);

    // // self->in_pos_cmd = self->step_controller.out_pulse_cmd * self->step_controller.step2rad * self->param_motion_dir;
    // // self->in_pos_cmd = self->step_controller.step * self->step_controller.step2rad * self->param_motion_dir;
    // self->in_pos_cmd = self->param_motion_dir > 0 ? (self->step_controller.step * self->step_controller.step2rad) : (BM_2PI - self->step_controller.step * self->step_controller.step2rad);

    // // self->EncoderCalibrationOfficial_Run(self);
    // self->controller->td.in_v = self->in_pos_cmd;
    // self->controller->td.Run(&self->controller->td);
    // self->in_pos_cmd = self->controller->td.out_x1;
    // self->in_spd_cmd = self->controller->td.out_x2;
    // self->in_acc_cmd = self->controller->td.out_g;

    // // /* motor ripple calibration */
    // // self->MotorRippleTableCalibrationTask(self);

    // /* zazen mode */
    // // self->ZazenModeRun(self);

    // /* run controller */
    // self->controller->in_encoder = self->encoder_fdb;
    // self->controller->in_pos_cmd = self->in_pos_cmd;
    // self->controller->in_spd_cmd = self->in_spd_cmd;
    // self->controller->in_acc_cmd = self->in_acc_cmd;
    // self->controller->in_pos_fdb = self->pos_fdb_encoder;
    // self->controller->in_spd_fdb = self->spd_fdb_encoder;
    // self->controller->in_elec_angle_fdb = self->elec_angle_fdb;
    // self->controller->in_ud_cmd = 0;
    // self->controller->in_uq_cmd = 0;
    // self->controller->Run(self->controller);

    // self->CheckMotorPhaseResistance(self);
    // self->SystemIdentification_RL(self);
    // // self->in_ualpha_cmd = 0.081667;
    // // self->in_ubeta_cmd = 0;

    // /* chopper run 0.4255us */
    // // self->chopper.in_Ualpha = 0.00;
    // // self->chopper.in_Ubeta = 0;
    // self->chopper.in_Ualpha = self->controller->foc.ualpha + self->in_ualpha_cmd;
    // self->chopper.in_Ubeta = self->controller->foc.ubeta + self->in_ubeta_cmd;
    // self->chopper.Output(&self->chopper);

    // /* set pwm 0.723us*/

    // if (self->param_phase_order_invert == 0)
    // {
    //     self->driver_board->in_pwm_duty_A1 = self->chopper.out_Duty_A1;
    //     self->driver_board->in_pwm_duty_A2 = self->chopper.out_Duty_A2;
    //     self->driver_board->in_pwm_duty_B1 = self->chopper.out_Duty_B1;
    //     self->driver_board->in_pwm_duty_B2 = self->chopper.out_Duty_B2;
    // }
    // else
    // {
    //     self->driver_board->in_pwm_duty_A1 = self->chopper.out_Duty_A1;
    //     self->driver_board->in_pwm_duty_A2 = self->chopper.out_Duty_A2;
    //     self->driver_board->in_pwm_duty_B1 = self->chopper.out_Duty_B2;
    //     self->driver_board->in_pwm_duty_B2 = self->chopper.out_Duty_B1;
    // }

    // self->driver_board->UpdatePWM(self->driver_board);

    // if (0 == self->timer_section_num)
    // {
    //     self->encoder_fdb_tmp = self->encoder_fdb_tim;
    //     // self->foc_theta_tmp = self->foc_theta_tim;
    // }//
    //
    /* 62ns */
    // self->timer_tick++;
    // if (self->timer_tick >= 0xffffffffU / self->n_multiple_of_timmer_tick * self->n_multiple_of_timmer_tick)
    // {
    //     self->timer_tick = 0;
    // }

    // self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);
}

void StepperMotorController_DriverEnableControl(struct StepperMotorController *self, uint16_t enable)
{
    if (enable != 0)
    {
        // self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "protect_err_code_backup");
        // if (self->protection.err_code_backup != 0)
        // {
        //     self->flash_manager_param->WriteVariableIntoFlash(self->flash_manager_param, "protect_err_code_backup", "0");
        //     self->protection.err_code_backup = 0;
        // }

        /* encoder solve */
        // self->param_mech_offset = 0; // self->encoder_fdb;

        /* pos cmd clear */
        self->step_controller.SetInitStepFromRad(&self->step_controller, self->encoder_fdb * self->encoder_2_pos_rad);
        // self->in_pos_cmd = self->step_controller.step * self->step_controller.step2rad * self->param_motion_dir;
        self->in_pos_cmd = self->param_motion_dir > 0 ? (self->step_controller.step * self->step_controller.step2rad) : (BM_2PI - self->step_controller.step * self->step_controller.step2rad);

        /* pwm output */
        self->driver_board->param_pwm_output_en = 1;

        /* control switch disable */
        self->controller->Clear(self->controller);
        self->controller->leso.Init_z1z2z3(&self->controller->leso, self->pos_fdb_encoder, 0, 0);
        self->controller->param_en_all_control_switch = 1;

        /* leso disable */
        self->controller->leso.param_en = 1;

        /* timmer start */
        self->mcu->SetInputPulseCounter(0);
        self->mcu->EnableInputPulseCounter();
        self->step_controller.pulse_count = 0;
        self->step_controller.pulse_count_last = 0;
    }
    else
    {
        /* pwm output */
        self->driver_board->param_pwm_output_en = 0;
        /* control switch disable */
        self->controller->param_en_all_control_switch = 0;
        /* leso disable */
        self->controller->leso.param_en = 0;
        /* timmer stop */
        self->mcu->DisableInputPulseCounter();
        self->mcu->SetInputPulseCounter(0);
        self->step_controller.pulse_count = 0;
        self->step_controller.pulse_count_last = 0;

        /* encoder solve */
        // self->param_mech_offset = 0; // self->encoder_fdb;

        /* pos cmd clear */
        self->step_controller.SetInitStepFromRad(&self->step_controller, self->encoder_fdb * self->encoder_2_pos_rad);
        // self->in_pos_cmd = self->step_controller.step * self->step_controller.step2rad * self->param_motion_dir;
        self->in_pos_cmd = self->param_motion_dir > 0 ? (self->step_controller.step * self->step_controller.step2rad) : (BM_2PI - self->step_controller.step * self->step_controller.step2rad);
    }
}

void StepperMotorController_UpdateParam(struct StepperMotorController *self)
{
    DISABLE_GLOBAL_INTERRUPT();

    PeriodCorrect(self);
    ParamFurtherSetup(self);

    self->zazen_mode.trigger_period_count = (uint32_t)(self->zazen_mode.param_trigger_time_s / self->zazen_mode.param_Ts);

    /* init mcu */
    // self->mcu->Init(self->mcu);

    /* init encoder */
    self->encoder->Init(self->encoder);

    /* init driver board */
    self->driver_board->Init(self->driver_board);

    /* init motor */
    self->stepper_motor->Init(self->stepper_motor);

    /* init controller */
    self->controller->Init(self->controller);

    /* init step_controller */
    self->step_controller.UpdateParam(&self->step_controller);

    /* init protection */
    self->protection.UpdateParam(&self->protection);

    /* init chopper */
    self->chopper.Init(&self->chopper);

    /* init logger */
    self->logger->Init(self->logger);

    ENABLE_GLOBAL_INTERRUPT();
}

void StepperMotorController_CheckMotorPhaseOrder(struct StepperMotorController *self, uint8_t debug_output)
{

    // const uint16 self->param_delay_ms_per_step = 10;
    // const float32 self->param_ud_cal_set = 0.10;
    int32 encoder_read[4] = {0};

    /* close protection */
    uint8 protect_tmp;
    protect_tmp = self->protection.param_en;
    self->protection.param_en = 0;

    if (debug_output != 0)
    {
        DBprintf("CheckMotorPhaseOrder start\r\n");
        DBprintf("CheckMotorPhaseOrder running......\r\n");
    }

    self->driver_board->param_pwm_output_en = 1;
    self->controller->foc.ud = 0;
    self->controller->foc.uq = 0;
    self->controller->foc.theta = 0;
    self->UdUqOutput(self);
    self->mcu->DelayMs(self->mcu, 500);

    /* init */
    self->controller->foc.ud = self->param_ud_cal_set;
    self->controller->foc.uq = 0;
    self->controller->foc.theta = 0;
    self->UdUqOutput(self);
    self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_step * 2);

    /* test */
    for (uint16_t i = 0; i < 4; i++)
    {
        self->controller->foc.ud = self->param_ud_cal_set;
        self->controller->foc.uq = 0;
        self->controller->foc.theta = self->param_calibration_dir * i * BM_PI_D2;
        self->UdUqOutput(self);
        self->mcu->DelayMs(self->mcu, self->param_delay_ms_per_step);
        encoder_read[i] = self->encoder->GetEncoder(self->encoder);

        if (debug_output != 0)
        {
            DBprintf("encoder_read[%d]: %ld\r\n", i, encoder_read[i]);
        }
    }

    self->driver_board->param_pwm_output_en = 0;
    self->controller->foc.ud = 0;
    self->controller->foc.uq = 0;
    self->controller->foc.theta = 0;
    self->UdUqOutput(self);

    /* close protection */
    self->protection.param_en = protect_tmp;

    /* check */
    if ((self->param_calibration_dir * encoder_read[0] < self->param_calibration_dir * encoder_read[1] &&
         self->param_calibration_dir * encoder_read[1] < self->param_calibration_dir * encoder_read[2] &&
         self->param_calibration_dir * encoder_read[2] < self->param_calibration_dir * encoder_read[3]))
    {
        self->param_phase_order_invert = 0;
    }
    else
    {
        self->param_phase_order_invert = 1;
    }

    if (debug_output != 0)
    {
        DBprintf("phase_order_invert: %d\r\n", self->param_phase_order_invert);
    }

    self->mcu->DelayMs(self->mcu, 100);
    self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "phase_order_invert");
}

void StepperMotorController_EncoderCalibrationOfficial_Run(struct StepperMotorController *self, uint8_t debug_output, uint8_t output_format)
{
    if (debug_output != 0)
    {
        DBprintf("ElecZeroOffset calibration start\r\n");
        DBprintf("ElecZeroOffset calibration running......\r\n");
    }

    // self->mcu->GpioReset(self->driver_board->param_pin_CAL_EN.pin);

    /* motor run */
    self->driver_board->param_pwm_output_en = 1;
    self->controller->foc.ud = 0;
    self->controller->foc.uq = 0;
    self->controller->foc.theta = 0;
    self->UdUqOutput(self);
    self->mcu->DelayMs(self->mcu, 500);

    float acc_ratio = 0.1;
    uint32 acc_count = (uint32)(acc_ratio * self->param_encoder_calibtate_official_cal_count_max * self->param_encoder_calibration_round_count);
    uint32 dec_count = self->param_encoder_calibtate_official_cal_count_max * self->param_encoder_calibration_round_count - acc_count;
    float ud_init_ratio = 0.5;
    uint16 speed_delay_us = 40;

    /* init */
    self->controller->foc.ud = ud_init_ratio * self->param_encoder_calibtate_official_ud_cal_set;
    self->controller->foc.uq = 0;
    self->controller->foc.theta = 0;
    self->UdUqOutput(self);
    self->mcu->DelayMs(self->mcu, 10);

    /* set calibration speed **************************/
    uint8_t write_buff[6] = {0x58};
    uint8_t read_buff[6] = {0};

    if (debug_output != 0)
    {
        DBprintf("-------set calibration speed...\r\n");
    }

    self->encoder->ReadReg(self->encoder, MT6835_REG_GPIO_DS__AUTOCAL_FREQ_2to0, read_buff, 1);

    if (debug_output != 0)
    {
        DBprintf("read buff: \r\n");

        for (size_t i = 0; i < 3; i++)
        {
            DBprintf("%#x ", read_buff[i]);
        }
        DBprintf("\r\n");
    }

    self->encoder->WriteReg(self->encoder, MT6835_REG_GPIO_DS__AUTOCAL_FREQ_2to0, write_buff, 1);

    if (debug_output != 0)
    {
        DBprintf("write buff: \r\n");
        for (size_t i = 0; i < 3; i++)
        {
            DBprintf("%#x ", write_buff[i]);
        }
        DBprintf("\r\n");
    }

    self->encoder->ReadReg(self->encoder, MT6835_REG_GPIO_DS__AUTOCAL_FREQ_2to0, read_buff, 1);

    if (debug_output != 0)
    {
        DBprintf("read buff: \r\n");
        for (size_t i = 0; i < 3; i++)
        {
            DBprintf("%#x ", read_buff[i]);
        }
        DBprintf("\r\n");
    }

    self->encoder->SaveIntoEEPROM(self->encoder);
    self->mcu->DelayMs(self->mcu, 6000);

    if (debug_output != 0)
    {
        DBprintf("EEPROM write finished!\r\n");

        /* calibration start *******************************/
        DBprintf("-------calibration start...\r\n");
    }

    self->controller->foc.ud = ud_init_ratio * self->param_encoder_calibtate_official_ud_cal_set;
    for (uint32 i = 0; i < self->param_encoder_calibtate_official_cal_count_max * self->param_encoder_calibration_round_count; i++)
    {
        self->controller->foc.uq = 0;
        self->controller->foc.theta = self->param_calibration_dir * fmodf(i * (BM_2PI * (float32)self->stepper_motor->param_pole_pair / (float32)self->param_encoder_calibtate_official_cal_count_max), BM_2PI);
        self->UdUqOutput(self);

        /* control delay */
        if (i < acc_count)
        {
            self->mcu->DelayUs(self->mcu, (uint16)(speed_delay_us * (1 - (float)i / acc_count) + speed_delay_us));
            self->controller->foc.ud = (1 - ud_init_ratio) * self->param_encoder_calibtate_official_ud_cal_set * ((float)i / acc_count) + ud_init_ratio * self->param_encoder_calibtate_official_ud_cal_set;
        }
        else if (i < dec_count)
        {
            // if (i == acc_count)
            // {
            /* cal en */
            //            self->mcu->GpioSet(self->driver_board->param_pin_CAL_EN.pin);
            // }

            self->mcu->DelayUs(self->mcu, speed_delay_us);
            self->controller->foc.ud = (self->param_encoder_calibtate_official_ud_cal_set);
        }
        else
        {
            self->mcu->DelayUs(self->mcu, (uint16)(speed_delay_us * ((float)(i - dec_count) / acc_count) + speed_delay_us));
            self->controller->foc.ud = (1 - ud_init_ratio) * self->param_encoder_calibtate_official_ud_cal_set * ((1 - (float)(i - dec_count) / acc_count)) + ud_init_ratio * self->param_encoder_calibtate_official_ud_cal_set;
        }
        self->encoder->calibration_result.all = self->encoder->GetEncoderCalibrationStatus(self->encoder);

        if (self->encoder->calibration_result.bit.status == EncoderCalibrationStatus_Success)
        {
            break;
        }

        if (debug_output != 0)
        {
            DBprintf("%s%%\r\n", float2str((float)(self->encoder->calibration_result.bit.progress) / 0xF * 100.0, 1));
        }
    }

    self->mcu->DelayMs(self->mcu, 6000);

    if (output_format == ResultOutputFormat_Str)
    {
        DBprintf("result: %#x\r\n", self->encoder->calibration_result.all);
    }
    else if (output_format == ResultOutputFormat_Hex)
    {
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->encoder->calibration_result.bit.status);
        self->SendFormatedArray(self);
    }

    if (debug_output != 0)
    {
        DBprintf("encoder calibrate finished!\r\n");
    }
}

int16_t StepperMotorController_AdcCalibration_Run(struct StepperMotorController *self)
{

    // self->adc_calibration_buff = malloc(self->param_adc_calibration_buff_size * sizeof(uint16_t));
    static uint16_t procedure_num = 0;
    static int32 ia_sum = 0;
    static int32 ib_sum = 0;
    static uint16_t sample_i = 0;

    switch (procedure_num)
    {
    case 0:
        ia_sum = 0;
        ib_sum = 0;
        sample_i = 0;
        procedure_num++;
        self->init_flag.bit.adc_calibration = 0;
        self->mcu->SetAllPwm(self->mcu, 1, 1, 1, 1);
        // DBprintf("p0\r\n");
        break;
    case 1:
        /* code */
        // self->mcu->ADC_StartConversion();
        self->mcu->ADC_WaitConversionComplete();
        if (sample_i >= self->driver_board->param_adc_calibration_start_index)
        {
            // ia_sum += self->mcu->AdcGetResult(ADC_NUM1, 1);
            // ib_sum += self->mcu->AdcGetResult(ADC_NUM2, 1);
            ia_sum += self->driver_board->Ia_adc_value;
            ib_sum += self->driver_board->Ib_adc_value;
            // DBprintf("pa\r\n");
        }
        else
        {

            // DBprintf("pb\r\n");
        }

        sample_i++;
        if (sample_i == self->driver_board->param_adc_calibration_start_index + self->driver_board->param_adc_calibration_buffer_size)
        {
            self->driver_board->param_Ia_adc_offset = ia_sum / self->driver_board->param_adc_calibration_buffer_size; // - self->mcu->adc1_offset;
            self->driver_board->param_Ib_adc_offset = ib_sum / self->driver_board->param_adc_calibration_buffer_size; // - self->mcu->adc2_offset;
            self->init_flag.bit.adc_calibration = 1;
            procedure_num++;
            // DBprintf("p1\r\n");
        }
        /* test */
        // self->driver_board->param_Ia_adc_offset = self->mcu->adc1_offset;
        // self->driver_board->param_Ib_adc_offset = self->mcu->adc2_offset;
        break;
    default:
        break;
    }

    return self->init_flag.bit.adc_calibration;
    // return -1;
}

uint16_t StepperMotorController_MotorRippleTableCalibrationTask(struct StepperMotorController *self)
{
    int32_t err;
    MotorRippleTableCalibrator *mrtc = &self->motor_ripple_table_calibrator;
    // if (1)
    if (mrtc->param_en)
    {

        switch (mrtc->procedure)
        {
        case MotorRippleTableCalibratorProgress_Init:
            self->DriverEnableControl(self, 1);
            mrtc->param_sample_times_per_pos = 10;
            mrtc->param_cal_table_size = 100;
            mrtc->param_delay_period = 5;
            mrtc->param_speed_step = 1;

            mrtc->pos_interval = self->encoder->param_encoder_max_pulse / mrtc->param_cal_table_size;
            mrtc->pos_step_encoder = 1;
            mrtc->pos_accumulate_encoder = 0;
            mrtc->tick_sample = 0;
            mrtc->tick_delay = 0;
            mrtc->pos_index = 0;
            mrtc->pos_encoder_sum = 0;
            mrtc->cur_A_sum = 0;

            mrtc->cur_A_buff = (float *)malloc(mrtc->param_cal_table_size * sizeof(float));
            mrtc->pos_encoder_buff = (int32_t *)malloc(mrtc->param_cal_table_size * sizeof(int32_t));

            // self->encoder_fdb = 5378;
            // self->encoder->param_encoder_max_pulse = 8000;

            mrtc->initial_pos_encoder = self->encoder_fdb;
            mrtc->pos_encoder = mrtc->initial_pos_encoder;
            mrtc->pos_encoder_landmark = mrtc->initial_pos_encoder;
            mrtc->pos_rad = mrtc->pos_encoder * self->encoder_2_pos_rad;
            mrtc->pos_step_accumulate = (float)(mrtc->pos_encoder);
            mrtc->pos_encoder = (int32_t)(mrtc->pos_step_accumulate) % self->encoder->param_encoder_max_pulse;

            /* get encoder ref */

            mrtc->procedure = MotorRippleTableCalibratorProgress_ForwardRotation;
            // mrtc->procedure = MotorRippleTableCalibratorProgress_Finished;
            DBprintf("-------------------ripple calibration start-------------------\r\n");
            DBprintf("initial_pos_encoder: %d\r\n", mrtc->initial_pos_encoder);
            DBprintf("pos_encoder: %d\r\n", mrtc->pos_encoder);
            DBprintf("pos_step_encoder: %d\r\n", mrtc->pos_step_encoder);
            DBprintf("pos_rad: %s\r\n", float2str(mrtc->pos_rad, 4));
            DBprintf("encoder_fdb: %d\r\n", self->encoder_fdb);
            DBprintf("pos_fdb_encoder: %s\r\n", float2str(self->pos_fdb_encoder, 4));
            DBprintf("pos_encoder_landmark: %d\r\n", mrtc->pos_encoder_landmark);
            DBprintf("pos_step_accumulate: %s\r\n", float2str(mrtc->pos_step_accumulate, 4));

            DBprintf(":::::::::::::::::::\r\n");
            DBprintf("%d\r\n", mrtc->pos_encoder);
            break;
        case MotorRippleTableCalibratorProgress_ForwardRotation:
            mrtc->pos_step_accumulate += mrtc->param_speed_step;
            mrtc->pos_encoder = (int32_t)(mrtc->pos_step_accumulate + self->encoder->param_encoder_max_pulse) % self->encoder->param_encoder_max_pulse;

            /* set next pos ref */
            mrtc->pos_rad = mrtc->pos_encoder * self->encoder_2_pos_rad;

            /*  */
            err = mrtc->pos_encoder - mrtc->pos_encoder_landmark;
            if (err < -(self->encoder->param_encoder_max_pulse >> 1))
            {
                err += self->encoder->param_encoder_max_pulse;
            }
            else if (err > (self->encoder->param_encoder_max_pulse >> 1))
            {
                err -= self->encoder->param_encoder_max_pulse;
            }

            if (abs_bm(err) >= mrtc->pos_interval)
            {
                // DBprintf("run finished\r\n");
                // DBprintf("%d\r\n", mrtc->pos_encoder);
                /* finished */
                mrtc->pos_encoder_landmark = (mrtc->initial_pos_encoder + mrtc->pos_interval * (mrtc->pos_index + 1)) % self->encoder->param_encoder_max_pulse;
                mrtc->procedure = MotorRippleTableCalibratorProgress_ForwardRotationDelay;
            }
            break;
        case MotorRippleTableCalibratorProgress_ForwardRotationDelay:
            // DBprintf("tick_delay: %d\r\n", mrtc->tick_delay);
            mrtc->tick_delay++;
            if (mrtc->tick_delay >= mrtc->param_delay_period)
            {
                mrtc->tick_delay = 0;
                mrtc->procedure = MotorRippleTableCalibratorProgress_ForwardRotationSample;
            }
            break;
        case MotorRippleTableCalibratorProgress_ForwardRotationSample:

            // DBprintf("tick_sample: %d\r\n", mrtc->tick_sample);

            mrtc->tick_sample = (mrtc->tick_sample + 1) % mrtc->param_sample_times_per_pos;

            mrtc->cur_A_sum += self->controller->foc.iq;
            mrtc->pos_encoder_sum += self->controller->in_encoder;
            if (mrtc->tick_sample == 0)
            {
                // DBprintf("pos_index:%d\r\n", mrtc->pos_index);
                /* get cur pos encoder */
                mrtc->cur_A_buff[mrtc->pos_index] = mrtc->cur_A_sum / mrtc->param_sample_times_per_pos;
                mrtc->pos_encoder_buff[mrtc->pos_index] = mrtc->pos_encoder_sum / mrtc->param_sample_times_per_pos;
                mrtc->cur_A_sum = 0;
                mrtc->pos_encoder_sum = 0;
                mrtc->pos_index++;
                if (mrtc->pos_index >= mrtc->param_cal_table_size)
                {
                    mrtc->procedure = MotorRippleTableCalibratorProgress_Finished;
                }
                else
                {
                    mrtc->procedure = MotorRippleTableCalibratorProgress_ForwardRotation;
                }
            }
            break;
        case MotorRippleTableCalibratorProgress_BackwardRotation:
            mrtc->pos_step_accumulate -= mrtc->param_speed_step;
            mrtc->pos_encoder = (int32_t)(mrtc->pos_step_accumulate + self->encoder->param_encoder_max_pulse) % self->encoder->param_encoder_max_pulse;

            /* set next pos ref */
            mrtc->pos_rad = mrtc->pos_encoder * self->encoder_2_pos_rad;

            /*  */
            err = mrtc->pos_encoder - mrtc->pos_encoder_landmark;
            if (err < -(self->encoder->param_encoder_max_pulse >> 1))
            {
                err += self->encoder->param_encoder_max_pulse;
            }
            else if (err > (self->encoder->param_encoder_max_pulse >> 1))
            {
                err -= self->encoder->param_encoder_max_pulse;
            }

            if (abs_bm(err) >= mrtc->pos_interval)
            {
                // DBprintf("run finished\r\n");
                // DBprintf("%d\r\n", mrtc->pos_encoder);
                /* finished */
                mrtc->pos_encoder_landmark = (mrtc->initial_pos_encoder + mrtc->pos_interval * (mrtc->pos_index + 1)) % self->encoder->param_encoder_max_pulse;
                mrtc->procedure = MotorRippleTableCalibratorProgress_ForwardRotationDelay;
            }
            break;
        case MotorRippleTableCalibratorProgress_BackwardRotationDelay:
            break;
        case MotorRippleTableCalibratorProgress_BackwardRotationSample:
            break;
        case MotorRippleTableCalibratorProgress_Finished:
            DBprintf("finished!\r\n");
            self->DriverEnableControl(self, 0);
            mrtc->param_en = 0;
            break;

        default:
            break;
        }
        self->in_pos_cmd = mrtc->pos_rad;
        self->in_spd_cmd = 0;
        self->in_acc_cmd = 0;
    }

    return 0;
}

void StepperMotorController_Cmd_help_Async_Task(struct StepperMotorController *self)
{
    static int16_t step = -1;
    const uint16_t max_step = 50;

    if (self->cmd_help_task_trigger != 0)
    {
        if (self->cmd_help_task_index < self->cmd_nums)
        {
            switch (step)
            {
            case -1:
                TERMINAL_AdvStr("All supported command:\r\n");
                step = 0;
                break;
            case 0:
                TERMINAL_AdvStr("    ");
                TERMINAL_Adv("%d.[", self->cmd_help_task_index);
                TERMINAL_AdvStr(self->cmd_table[self->cmd_help_task_index].name);
                TERMINAL_AdvStr("]: ");
                step = (step + 1) % max_step;
                break;
            case 1 ... 2:
                step = (step + 1) % max_step;
                break;
            case 3:
                TERMINAL_AdvStr(self->cmd_table[self->cmd_help_task_index].description);
                TERMINAL_AdvStr("\r\n");
                self->cmd_help_task_index++;
                step = (step + 1) % max_step;
                break;
            case 4 ... 49:
                step = (step + 1) % max_step;
                break;
            default:
                break;
            }
        }
        else
        {
            self->cmd_help_task_trigger = 0;
            self->cmd_help_task_index = 0;
            step = -1;
        }
    }
}

void StepperMotorController_Cmd_helpCmd_Async_Task(struct StepperMotorController *self)
{
    static int16_t step = 0;
    static int16_t str_len = 0;
    static int16_t index = 0;
    static int16_t send_len = 0;
    static int16_t i;

    if (self->cmd_helpcmd_task_trigger != 0)
    {
        switch (step)
        {
        case 0:
            TERMINAL_Adv("[");
            TERMINAL_AdvStr(self->cmd_table[self->cmd_helpcmd_task_cur_index].name);
            TERMINAL_AdvStr("]: ");
            break;
        case 1 ... 2:
            break;
        case 3:
            TERMINAL_AdvStr(self->cmd_table[self->cmd_helpcmd_task_cur_index].description);
            TERMINAL_AdvStr("\r\n");
            break;
        case 4 ... 9:
            break;
        case 10:
            str_len = strlen(self->cmd_table[self->cmd_helpcmd_task_cur_index].usage);
            index = 0;
            send_len = str_len / 30;
            i = 0;
            break;
        case 11:
            if (index < str_len)
            {
                if (i == 0)
                {
                    TERMINAL_AdvStr_N(&self->cmd_table[self->cmd_helpcmd_task_cur_index].usage[index], send_len);
                    index += send_len;
                }
                i = (i + 1) % 5;
                step--;
            }
            break;
        case 12:
            TERMINAL_AdvStr("\r\n");
            break;
        case 13 ... 15:
            // TERMINAL_AdvStr("==\r\n");
            break;
        default:
            self->cmd_helpcmd_task_trigger = 0;
            self->cmd_helpcmd_task_cur_index = 0;
            step = -1;
            break;
        }
        step = step + 1;
    }
}

void StepperMotorController_RegisterCmdToTable(struct StepperMotorController *self, uint16_t data_num, uint8_t id, uint8_t broadcast_support, char *name, char *description, char *usage, void (*func)(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]))
{
    self->cmd_table[data_num].id = id;
    self->cmd_table[data_num].broadcast_support = broadcast_support;
    self->cmd_table[data_num].name = name;
    self->cmd_table[data_num].description = description;
    self->cmd_table[data_num].usage = usage;
    self->cmd_table[data_num].func = func;
    self->cmd_nums++;
}

void StepperMotorController_SystemIdentification_RL(struct StepperMotorController *self)
{
    uint16_t i;
    float value_last;
    static float *output_phase;
    static float *current_fdb;

    // self->system_id_RL.start = 1;
    if (self->system_id_RL.start != 0)
    {
        switch (self->system_id_RL.progress)
        {
        case SystemId_RL_Progress_PreparationStart:
            self->ProtectionCheckWarning(self, WARN_NUM_MOTOR_POWER_SUPPLY_TOO_LOW);
            if (self->protection.warning_code_raw & PROTECTION_CODE(WARN_NUM_MOTOR_POWER_SUPPLY_TOO_LOW))
            {
                self->system_id_RL.failure = SystemId_RL_Failure_LowPower;
                self->system_id_RL.progress = SystemId_RL_Progress_Failed;
                break;
            }

            // DBprintf("\r\n");
            self->system_id_RL.finished = 0;
            self->system_id_RL.failure = SystemId_RL_Failure_NoFailure;

            self->in_ualpha_cmd = 0;
            self->in_ubeta_cmd = 0;

            /* Choose phase */
            if (self->system_id_RL.in_phase_num == 0)
            {
                output_phase = &self->in_ualpha_cmd;
                current_fdb = &self->controller->in_ialpha_fdb;
            }
            else if (self->system_id_RL.in_phase_num == 1)
            {
                output_phase = &self->in_ubeta_cmd;
                current_fdb = &self->controller->in_ibeta_fdb;
            }

            /*  */
            self->system_id_RL.uin = self->system_id_RL.param_uin_series[0];
            // self->in_ualpha_cmd = self->system_id_RL.uin / self->driver_board->param_power_supply_V;

            /* clear controller */
            self->controller->param_en_all_control_switch = 0;
            self->controller->Clear(self->controller);

            *output_phase = self->system_id_RL.uin;

            /* enable pwm switch */
            self->driver_board->param_pwm_output_en = 1;

            self->system_id_RL.prepare_count = 0;

            self->system_id_RL.progress = SystemId_RL_Progress_PreparationRun;
            break;
        case SystemId_RL_Progress_PreparationRun:
            if (self->system_id_RL.prepare_count < (self->system_id_RL.param_prepare_time >> 1))
            {
                self->system_id_RL.prepare_count++;
            }
            else if (self->system_id_RL.prepare_count < self->system_id_RL.param_prepare_time)
            {
                *output_phase = 0;
                self->system_id_RL.prepare_count++;
            }
            else
            {
                self->system_id_RL.progress = SystemId_RL_Progress_UinTestStart;
            }
            break;
        case SystemId_RL_Progress_UinTestStart:

            /* reset current buff */
            self->system_id_RL.current_buff = (float *)malloc(self->system_id_RL.param_current_buff_size * sizeof(float));
            memset(self->system_id_RL.current_buff, 0, self->system_id_RL.param_current_buff_size);
            self->system_id_RL.current_index = 0;

            /* init current err buff */
            self->system_id_RL.current_err_buff = (float *)malloc(self->system_id_RL.param_current_err_size * sizeof(float));
            memset(self->system_id_RL.current_err_buff, 0, self->system_id_RL.param_current_err_size);

            /* clear controller */
            // self->controller->param_en_all_control_switch = 0;
            // self->controller->Clear(self->controller);

            *output_phase = self->system_id_RL.uin;
            self->system_id_RL.current_buff[0] = *current_fdb;

            /* enable pwm switch */
            // self->driver_board->param_pwm_output_en = 1;

            // TERMINAL_Adv("%d\t%s\r\n", float2str(self->system_id_RL.current_index, 3), float2str(self->system_id_RL.current_buff[self->system_id_RL.current_index], 3));

            self->system_id_RL.current_index = 1;
            self->system_id_RL.progress = SystemId_RL_Progress_UinTest;
            break;
        case SystemId_RL_Progress_UinTest:
            if (self->system_id_RL.current_index < self->system_id_RL.param_current_buff_size)
            {
                self->system_id_RL.current_buff[self->system_id_RL.current_index] = *current_fdb;
                // TERMINAL_Adv("%d\t%s\r\n", float2str(self->system_id_RL.current_index, 3), float2str(self->system_id_RL.current_buff[self->system_id_RL.current_index], 3));
                self->system_id_RL.current_index++;
            }
            else
            {
                self->in_ualpha_cmd = 0;
                self->in_ubeta_cmd = 0;
                self->driver_board->param_pwm_output_en = 0;
                self->system_id_RL.progress = SystemId_RL_Progress_Calculate;
                // DBprintf("-=-cal\r\n");
            }
            break;
        case SystemId_RL_Progress_Calculate:
            /* looking for steady time */
            value_last = self->system_id_RL.current_buff[0];
            self->system_id_RL.is_stable = 0;
            for (i = 1; i < self->system_id_RL.param_current_buff_size; i++)
            {
                for (uint16_t j = (int16_t)(self->system_id_RL.param_current_err_size - 1); j > 0; j--)
                {
                    self->system_id_RL.current_err_buff[j] = self->system_id_RL.current_err_buff[j - 1];
                }
                self->system_id_RL.current_err_buff[0] = self->system_id_RL.current_buff[i] - value_last;
                value_last = self->system_id_RL.current_buff[i];
                // DBprintf("%s %s %s\r\n", float2str(self->system_id_RL.current_err_buff[0], 4),float2str(self->system_id_RL.current_err_buff[1], 4),float2str(self->system_id_RL.current_err_buff[2], 4));

                float err_sum = 0;
                uint16_t is_all_positive = 1;
                uint16_t err_cond_ok = 0;
                for (uint16_t k = 0; k < self->system_id_RL.param_current_err_size; k++)
                {
                    err_sum += self->system_id_RL.current_err_buff[k];
                    if (self->system_id_RL.current_err_buff[k] < 0)
                    {
                        is_all_positive = 0;
                    }
                }
                if (is_all_positive == 0 &&
                    i > self->system_id_RL.param_current_err_size &&
                    abs_bm(err_sum) < self->system_id_RL.param_current_stable_err_condition)
                {
                    uint16_t i_ok;
                    // i_ok = i - self->system_id_RL.param_current_err_size;
                    // i_ok = i + self->system_id_RL.param_current_err_size>>1;
                    i_ok = i;
                    self->system_id_RL.final_value = self->system_id_RL.current_buff[i_ok];
                    self->system_id_RL.tf = i_ok * self->system_id_RL.param_Ts;
                    self->system_id_RL.is_stable = 1;

                    if (self->system_id_RL.in_debug_info_output != 0)
                    {
                        DBprintf("i_final:%d\r\n", i_ok);
                        DBprintf("tf_final:%s\r\n", float2str(self->system_id_RL.tf, 4));
                        DBprintf("final_value:%s\r\n", float2str(self->system_id_RL.final_value, 4));
                    }
                }

                if (self->system_id_RL.is_stable != 0)
                {
                    break;
                }
            }

            if (self->system_id_RL.in_debug_info_output != 0)
            {
                DBprintf("break_i:%d\r\n", i - 1);
                DBprintf("break_cur:%s\r\n", float2str(self->system_id_RL.current_buff[i - 1], 4));
                DBprintf("break_is_stable:%d\r\n", self->system_id_RL.is_stable);
                DBprintf("break_final_value:%s\r\n", float2str(self->system_id_RL.final_value, 4));
            }

            for (i = 0; i < self->system_id_RL.param_current_buff_size; i++)
            {
                if (self->system_id_RL.current_buff[i] >= 0.5 * self->system_id_RL.final_value)
                {
                    self->system_id_RL.td = i * self->system_id_RL.param_Ts;
                    self->system_id_RL.Tc = self->system_id_RL.td / 0.69;
                    self->system_id_RL.tr = 2.2 * self->system_id_RL.Tc;
                    self->system_id_RL.ts = 3.0 * self->system_id_RL.Tc;
                    self->system_id_RL.R = (self->system_id_RL.uin -
                                            (self->driver_board->param_mos_deadtime * 2 + self->driver_board->param_mos_rise_fall_time * 2) /
                                                self->system_id_RL.param_Ts) *
                                               self->driver_board->param_power_supply_V /
                                               self->system_id_RL.final_value -
                                           self->driver_board->param_mos_Ron;
                    self->system_id_RL.L = self->system_id_RL.Tc * self->system_id_RL.R;

                    if (self->system_id_RL.in_debug_info_output != 0)
                    {
                        DBprintf("i_td:%d\r\n", i);
                        DBprintf("Tc:%s\r\n", float2str(self->system_id_RL.Tc, 4));
                        DBprintf("td:%s\r\n", float2str(self->system_id_RL.td, 4));
                        DBprintf("tr:%s\r\n", float2str(self->system_id_RL.tr, 4));
                        DBprintf("ts:%s\r\n", float2str(self->system_id_RL.ts, 4));
                    }

                    if (self->system_id_RL.in_result_output_format == ResultOutputFormat_Str)
                    {
                        DBprintf("\r\nR(ohm):%s L(mH):%s\r\n", float2str(self->system_id_RL.R, 4), float2str(self->system_id_RL.L * 1e3, 4));
                    }
                    else if (self->system_id_RL.in_result_output_format == ResultOutputFormat_Hex)
                    {
                        data_conversion.f32 = self->system_id_RL.R;
                        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 0) & 0xff);
                        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 8) & 0xff);
                        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 16) & 0xff);
                        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 24) & 0xff);
                        data_conversion.f32 = self->system_id_RL.L;
                        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 0) & 0xff);
                        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 8) & 0xff);
                        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 16) & 0xff);
                        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 24) & 0xff);
                        self->SendFormatedArray(self);
                    }

                    break;
                }
            }

            /* Check if motor is connected */
            if (self->system_id_RL.is_stable == 1 &&
                abs_bm(self->system_id_RL.final_value) < 0.1 &&
                i < 10)
            {
                // DBprintf("i_failed:%d\r\n", i);
                self->system_id_RL.failure = SystemId_RL_Failure_UnConnected;
                self->system_id_RL.progress = SystemId_RL_Progress_Failed;
                break;
            }

            /* Check if stable */
            if (self->system_id_RL.is_stable == 0)
            {
                // DBprintf("i_failed:%d\r\n", i);
                self->system_id_RL.failure = SystemId_RL_Failure_UnStable;
                self->system_id_RL.progress = SystemId_RL_Progress_Failed;
                break;
            }

            /* Check if value is abnormal */
            if (self->system_id_RL.R < self->system_id_RL.param_normal_R_range_min ||
                self->system_id_RL.R > self->system_id_RL.param_normal_R_range_max ||
                self->system_id_RL.L < self->system_id_RL.param_normal_L_range_min ||
                self->system_id_RL.L > self->system_id_RL.param_normal_L_range_max)
            {
                // DBprintf("i_failed:%d\r\n", i);
                self->system_id_RL.failure = SystemId_RL_Failure_ValueAbnormal;
                self->system_id_RL.progress = SystemId_RL_Progress_Failed;
                break;
            }

            self->system_id_RL.progress = SystemId_RL_Progress_End;
            break;
        case SystemId_RL_Progress_End:
            if (self->system_id_RL.current_buff != NULL)
            {
                free(self->system_id_RL.current_buff);
            }
            if (self->system_id_RL.current_err_buff != NULL)
            {
                free(self->system_id_RL.current_err_buff);
            }

            self->system_id_RL.start = 0;
            self->system_id_RL.finished = 1;
            self->system_id_RL.progress = SystemId_RL_Progress_PreparationStart;
            break;
        case SystemId_RL_Progress_Failed:
            if (self->system_id_RL.current_buff != NULL)
            {
                free(self->system_id_RL.current_buff);
            }
            if (self->system_id_RL.current_err_buff != NULL)
            {
                free(self->system_id_RL.current_err_buff);
            }

            // DBprintf("failed, code: %d\r\n", self->system_id_RL.failure);
            self->system_id_RL.start = 0;
            self->system_id_RL.finished = 1;
            self->system_id_RL.progress = SystemId_RL_Progress_PreparationStart;
            break;
        default:
            break;
        }
    }
    else
    {
    }
}

void StepperMotorController_SystemIdentification_Ke(struct StepperMotorController *self)
{
}

void StepperMotorController_SystemIdentification_JB(struct StepperMotorController *self)
{
}

void StepperMotorController_SystemIdentification_Kt(struct StepperMotorController *self)
{
}

void StepperMotorController_CheckMotorPhaseResistance(struct StepperMotorController *self)
{
    static uint8_t try_count = 0;
    float R_tmp, L_tmp;

    if (self->motor_phase_check.start == 1)
    {
        switch (self->motor_phase_check.progress)
        {
        case 0:

            self->protection.err_code_raw &= ~(PROTECTION_CODE(ERR_NUM_MOTOR_CHECK_ValueAbnormal) |
                                               PROTECTION_CODE(ERR_NUM_MOTOR_CHECK_UnStable) |
                                               PROTECTION_CODE(ERR_NUM_MOTOR_CHECK_MotorPhaseNotConnected) |
                                               PROTECTION_CODE(ERR_NUM_MOTOR_CHECK_MotorPhaseResistanceDifferent) |
                                               PROTECTION_CODE(ERR_NUM_MOTOR_CHECK_OtherErr));
            self->protection.err_code = self->protection.err_code_raw;

            self->driver_board->GetPowerVoltage(self->driver_board);
            self->ProtectionCheckWarning(self, WARN_NUM_MOTOR_POWER_SUPPLY_TOO_LOW);
            if (self->protection.warning_code_raw & PROTECTION_CODE(WARN_NUM_MOTOR_POWER_SUPPLY_TOO_LOW))
            {
                self->motor_phase_check.failure = MotorPhaseCheck_Failure_LowPower;
                self->motor_phase_check.progress = 9;
                break;
            }
            self->DriverEnableControl(self, 0);
            self->motor_phase_check.finished = 0;
            self->motor_phase_check.failure = MotorPhaseCheck_Failure_NoFailure;
            try_count = 0;
            self->motor_phase_check.progress = 1;
            break;
        case 1:
            if (self->motor_phase_check.in_debug_info_output != 0)
            {
                DBprintf("\r\n[Aphase]: %d\r\n", try_count);
            }
            self->system_id_RL.start = 1;
            self->system_id_RL.in_phase_num = 0;
            self->system_id_RL.in_debug_info_output = self->motor_phase_check.in_debug_info_output;
            self->system_id_RL.in_result_output_format = self->motor_phase_check.in_result_output_format; // ResultOutputFormat_NoOutput;
            self->motor_phase_check.progress = 2;
            break;
        case 2:
            if (self->system_id_RL.finished != 0)
            {
                if (self->system_id_RL.failure == SystemId_RL_Failure_NoFailure)
                {
                    self->motor_phase_check.R_A = self->system_id_RL.R;
                    self->motor_phase_check.L_A = self->system_id_RL.L;
                    self->motor_phase_check.progress = 3;
                }
                else
                {
                    if (try_count < 2)
                    {
                        try_count++;
                        self->motor_phase_check.progress = 1;
                    }
                    else
                    {
                        /* Check if the RL value is within normal range */
                        if (self->system_id_RL.failure == SystemId_RL_Failure_LowPower)
                        {
                            self->motor_phase_check.failure = MotorPhaseCheck_Failure_LowPower;
                        }
                        else if (self->system_id_RL.failure == SystemId_RL_Failure_ValueAbnormal)
                        {
                            self->motor_phase_check.failure = MotorPhaseCheck_Failure_ValueAbnormal;
                        }
                        else if (self->system_id_RL.failure == SystemId_RL_Failure_UnStable)
                        {
                            self->motor_phase_check.failure = MotorPhaseCheck_Failure_UnStable;
                        }
                        else if (self->system_id_RL.failure == SystemId_RL_Failure_UnConnected)
                        {
                            self->motor_phase_check.failure = MotorPhaseCheck_Failure_MotorPhaseNotConnected;
                        }
                        else
                        {
                            self->motor_phase_check.failure = MotorPhaseCheck_Failure_OtherErr;
                        }
                        self->motor_phase_check.progress = 9;
                    }
                }
            }
            break;
        case 3:
            try_count = 0;
            self->motor_phase_check.progress = 4;
            break;
        case 4:
            if (self->motor_phase_check.in_debug_info_output != 0)
            {
                DBprintf("\r\n[Bphase]: %d\r\n", try_count);
            }
            self->system_id_RL.start = 1;
            self->system_id_RL.in_phase_num = 1;
            self->system_id_RL.in_debug_info_output = self->motor_phase_check.in_debug_info_output;
            self->system_id_RL.in_result_output_format = self->motor_phase_check.in_result_output_format;
            self->motor_phase_check.progress = 5;
            break;
        case 5:
            if (self->system_id_RL.finished != 0)
            {
                if (self->system_id_RL.failure == SystemId_RL_Failure_NoFailure)
                {
                    self->motor_phase_check.R_B = self->system_id_RL.R;
                    self->motor_phase_check.L_B = self->system_id_RL.L;
                    self->motor_phase_check.progress = 6;
                }
                else
                {
                    if (try_count < 2)
                    {
                        try_count++;
                        self->motor_phase_check.progress = 4;
                    }
                    else
                    {
                        /* Check if the RL value is within normal range */
                        if (self->system_id_RL.failure == SystemId_RL_Failure_LowPower)
                        {
                            self->motor_phase_check.failure = MotorPhaseCheck_Failure_LowPower;
                        }
                        else if (self->system_id_RL.failure == SystemId_RL_Failure_ValueAbnormal)
                        {
                            self->motor_phase_check.failure = MotorPhaseCheck_Failure_ValueAbnormal;
                        }
                        else if (self->system_id_RL.failure == SystemId_RL_Failure_UnStable)
                        {
                            self->motor_phase_check.failure = MotorPhaseCheck_Failure_UnStable;
                        }
                        else if (self->system_id_RL.failure == SystemId_RL_Failure_UnConnected)
                        {
                            self->motor_phase_check.failure = MotorPhaseCheck_Failure_MotorPhaseNotConnected;
                        }
                        else
                        {
                            self->motor_phase_check.failure = MotorPhaseCheck_Failure_OtherErr;
                        }
                        self->motor_phase_check.progress = 9;
                    }
                }
            }
            break;
        case 6:
            /* Cal */
            R_tmp = (self->motor_phase_check.R_A + self->motor_phase_check.R_B) / 2;
            L_tmp = (self->motor_phase_check.L_A + self->motor_phase_check.L_B) / 2;

            /* Check if the err between the resistance of two phase are equal within margin */
            self->motor_phase_check.motor_resistance_tolerance = abs_bm((self->motor_phase_check.R_A - self->motor_phase_check.R_B) / self->stepper_motor->param_R);
            if (self->motor_phase_check.motor_resistance_tolerance > self->motor_phase_check.param_motor_resistance_tolerance_standar_percent * 0.01)
            {

                if (self->motor_phase_check.in_debug_info_output != 0)
                {

                    DBprintf("\r\nressistance err(%%): %s\r\n", float2str(self->motor_phase_check.motor_resistance_tolerance * 100, 4));
                }
                self->motor_phase_check.failure = MotorPhaseCheck_Failure_MotorPhaseResistanceDifferent;
                self->motor_phase_check.progress = 9;
                return;
            }

            if (self->motor_phase_check.in_result_output_format == ResultOutputFormat_Str)
            {
                DBprintf("\r\nR(ohm): %s | L(mH): %s\r\n", float2str(R_tmp, 4), float2str(L_tmp * 1000, 4));
            }
            else if (self->motor_phase_check.in_result_output_format == ResultOutputFormat_Hex)
            {
                data_conversion.f32 = R_tmp;
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 0) & 0xff);
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 8) & 0xff);
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 16) & 0xff);
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 24) & 0xff);
                data_conversion.f32 = L_tmp;
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 0) & 0xff);
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 8) & 0xff);
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 16) & 0xff);
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 24) & 0xff);
                self->SendFormatedArray(self);
            }

            /* Save */
            if (self->motor_phase_check.in_save_into_flash_after_run != 0)
            {
                self->stepper_motor->param_R = R_tmp;
                self->stepper_motor->param_L = L_tmp;
                self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "motor_R");
                self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "motor_L");
            }

            self->UpdateParam(self);

            if (self->motor_phase_check.in_debug_info_output != 0)
            {
                DBprintf("\r\nfinished!\r\n");
            }
            self->motor_phase_check.finished = 1;
            self->motor_phase_check.progress = 0;
            self->motor_phase_check.start = 0;
            break;
        case 9:
            if (self->motor_phase_check.in_debug_info_output != 0)
            {
                DBprintf("\r\nfailed code: %d  ", self->motor_phase_check.failure);
                switch (self->motor_phase_check.failure)
                {
                case MotorPhaseCheck_Failure_LowPower:
                    DBprintf("LowPower\r\n");
                    break;
                case MotorPhaseCheck_Failure_ValueAbnormal:
                    DBprintf("ValueAbnormal\r\n");
                    break;
                case MotorPhaseCheck_Failure_UnStable:
                    DBprintf("UnStable\r\n");
                    break;
                case MotorPhaseCheck_Failure_MotorPhaseNotConnected:
                    DBprintf("MotorPhaseNotConnected\r\n");
                    break;
                case MotorPhaseCheck_Failure_MotorPhaseResistanceDifferent:
                    DBprintf("MotorPhaseResistanceDifferent\r\n");
                    break;
                case MotorPhaseCheck_Failure_OtherErr:
                    DBprintf("OtherErr\r\n");
                    break;
                default:
                    break;
                }
            }
            self->motor_phase_check.finished = 1;
            // self->motor_phase_check.failure = 1;
            self->motor_phase_check.progress = 0;
            self->motor_phase_check.start = 0;
            break;
        default:
            break;
        }
    }
}

void StepperMotorController_VersionPrintTask(struct StepperMotorController *self)
{
    static uint16_t process = 0;

    if (self->version_print_start == 1)
    {
        self->version_print_start = 0;
        process = 1;
    }
    switch (process)
    {
    case 1:
        TERMINAL_Adv("fireware ver: %d.%d.%d.%d | ", self->param_firmware_version.number.byte.major,
                     self->param_firmware_version.number.byte.minor,
                     self->param_firmware_version.number.byte.patch,
                     self->param_firmware_version.number.byte.compiled);
        process++;
        break;
    case 2 ...(2 + 3):
        process++;
        break;
    case 6:
        TERMINAL_Adv(" %s\r\n", self->param_firmware_version.release_date);
        process++;
        break;
    case 7 ...(7 + 3):
        process++;
        break;
    case 11:
        TERMINAL_Adv("cmd_system ver: %d.%d | ", self->communication_interface.param_version.number.byte.major,
                     self->communication_interface.param_version.number.byte.minor);
        process++;
        break;
    case 12 ...(12 + 3):
        process++;
        break;
    case 16:
        TERMINAL_Adv(" %s\r\n", self->communication_interface.param_version.release_date);
        process++;
        break;
    case 17 ...(17 + 3):
        process++;
        break;
    case 21:
        TERMINAL_Adv("protection ver: %d.%d | ", self->protection.param_version.number.byte.major,
                     self->protection.param_version.number.byte.minor);
        process++;
        break;
    case 22 ...(22 + 3):
        process++;
        break;
    case 26:
        TERMINAL_Adv(" %s\r\n", self->protection.param_version.release_date);
        process++;
        break;
    case 27 ...(27 + 3):
        process++;
        break;
    case 31:
        TERMINAL_Adv("flash_param ver: %d.%d | ", self->flash_manager_param->param_version.number.byte.major,
                     self->flash_manager_param->param_version.number.byte.minor);
        process++;
        break;
    case 32 ...(32 + 3):
        process++;
        break;
    case 36:
        TERMINAL_Adv(" %s\r\n", self->flash_manager_param->param_version.release_date);
        process++;
        break;
    case 37 ...(37 + 3):
        process++;
        break;
    case 41:
        TERMINAL_Adv("logger ver: %d.%d | ", self->logger->param_version.number.byte.major,
                     self->logger->param_version.number.byte.minor);
        process++;
        break;
    case 42 ...(42 + 3):
        process++;
        break;
    case 46:
        TERMINAL_Adv(" %s\r\n", self->logger->param_version.release_date);
        process++;
        break;
    default:
        break;
    }
}

void StepperMotorController_ZazenModeRun(struct StepperMotorController *self)
{
    if (self->zazen_mode.param_en != 0)
    {
        if (self->step_controller.pulse_count != self->step_controller.pulse_count_last)
        {
            self->zazen_mode.time_count = 0;
            self->ZazenModeEnterOrExit(self, 0);
        }
        else
        {
            if (self->zazen_mode.time_count == self->zazen_mode.trigger_period_count)
            {
                self->ZazenModeEnterOrExit(self, 1);
            }
            else
            {
                self->zazen_mode.time_count++;
            }
        }
    }
}

void StepperMotorController_ZazenModeEnterOrExit(struct StepperMotorController *self, uint16_t enter)
{
    if (enter)
    {
        /* enter zazen */
        self->controller->pos_loop_pid.kp = self->zazen_mode.param_gain.pos_kp * self->controller->pos_loop_pid.param_kp;
        self->controller->spd_loop_pid.kp = self->zazen_mode.param_gain.spd_kp * self->controller->spd_loop_pid.param_kp;
        self->controller->spd_loop_pid.ki = self->zazen_mode.param_gain.spd_ki * self->controller->spd_loop_pid.param_ki;
        self->controller->id_loop_pid.kp = self->zazen_mode.param_gain.cur_kp * self->controller->id_loop_pid.param_kp;
        self->controller->id_loop_pid.ki = self->zazen_mode.param_gain.cur_ki * self->controller->id_loop_pid.param_ki;
        self->controller->iq_loop_pid.kp = self->zazen_mode.param_gain.cur_kp * self->controller->iq_loop_pid.param_kp;
        self->controller->iq_loop_pid.ki = self->zazen_mode.param_gain.cur_ki * self->controller->iq_loop_pid.param_ki;
    }
    else
    {
        /* exit zazen */
        self->controller->pos_loop_pid.kp = self->controller->pos_loop_pid.param_kp;
        self->controller->spd_loop_pid.kp = self->controller->spd_loop_pid.param_kp;
        self->controller->spd_loop_pid.ki = self->controller->spd_loop_pid.param_ki;
        self->controller->id_loop_pid.kp = self->controller->id_loop_pid.param_kp;
        self->controller->id_loop_pid.ki = self->controller->id_loop_pid.param_ki;
        self->controller->iq_loop_pid.kp = self->controller->iq_loop_pid.param_kp;
        self->controller->iq_loop_pid.ki = self->controller->iq_loop_pid.param_ki;
    }
}

void StepperMotorController_SendFormatedArray(struct StepperMotorController *self)
{
    uint8_t *start_addr;

    /* Head */
    HexAddByte(DataFrame_Head);

    /* 485 Addr */
    HexAddByte(self->rs485_addr);

    /* Recording address */
    start_addr = &DebugNewDataBuff[DebugNewDataBuff_Len];

    /* Data length */
    HexAddByte((uint8_t)(self->communication_interface.send_data_array_len + 3));

    /* Status */
    HexAddByte(
        (uint8_t)(self->stall_is_trigger > 0) |
        (uint8_t)(self->protection.err_code != 0 ? 1 : 0) << 1 |
        (uint8_t)(self->protection.warning_code != 0 ? 1 : 0) << 2);

    /* FunctionCode */
    HexAddByte(self->communication_interface.data_info.function_code);

    /* Data */
    HexAddArray(self->communication_interface.send_data_array, self->communication_interface.send_data_array_len);

    /* Parity */
    self->communication_interface.data_info.parity_send = crc8_cal(start_addr, self->communication_interface.send_data_array_len + 3);
    HexAddByte(self->communication_interface.data_info.parity_send);

    /* Reset sending array */
    self->communication_interface.send_data_array_len = 0;
}

__attribute__((section(".highcode"))) void CmdFunction_help(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    // DBprintf("%s func\r\n", cmd_and_arg[0]);

    // function_code
    if (self->communication_interface.is_hex_frame == TRUE)
    {
        if (self->communication_interface.data_info.function_code == FunctionCmdCode2st_help)
        {
            self->cmd_help_task_trigger = 1;
        }
        else if (self->communication_interface.data_info.function_code < self->cmd_nums)
        {
            self->cmd_helpcmd_task_trigger = 1;
            self->cmd_helpcmd_task_cur_index = self->communication_interface.data_info.function_code;
        }
    }
    else
    {
        /* help show cmd detail */
        size_t i;
        for (i = 0; i < self->cmd_nums; i++)
        {
            if (strcmp((const char *)cmd_and_arg[1], self->cmd_table[i].name) == 0)
            {
                self->cmd_helpcmd_task_trigger = 1;
                self->cmd_helpcmd_task_cur_index = i;
                break;
            }
        }

        /* help show all */
        if (i == self->cmd_nums)
        {
            self->cmd_help_task_trigger = 1;
        }
    }
}

__attribute__((section(".highcode"))) void CmdFunction_reboot(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    // DBprintf("%s func\r\n", cmd_and_arg[0]);

    if (self->communication_interface.is_hex_frame == TRUE)
    {
        switch (self->communication_interface.data_info.data[0])
        {
        case FunctionCmdCode2st_reboot_null:
            self->SystemReboot(self);
            break;
        }
    }
    else
    {
        TERMINAL_AdvStr("reboot\r\n");
        self->SystemReboot(self);
    }
}

__attribute__((section(".highcode"))) void CmdFunction_encoder_calibrate(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    // StepperMotorController *self = (StepperMotorController *)smc;
    // // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // // uint8_t function_code = raw_data[3];

    // // DBprintf("%s func\r\n", cmd_and_arg[0]);

    // if (self->communication_interface.is_hex_frame == TRUE)
    // {
    //     // TERMINAL_AdvStr("encoder_calibrate\r\n");

    //     self->mcu->DisableNvicInterrupt();

    //     // self->mcu->param_uart_baudrate = self->mcu->param_uart_baudrate_encoder_calibration;
    //     self->mcu->InitUart(self->mcu);

    //     // TERMINAL_AdvStr("----[CALIBRATION] Encoder calibration enable\r\n");

    //     self->init_flag.bit.encoder_calibration = self->EncoderCalibration(self, 0);

    //     self->driver_board->param_pwm_output_en = 0;
    //     self->controller->param_en_all_control_switch = 0;

    //     self->mcu->EnableNvicInterrupt();
    // }
    // else
    // {
    //     // TERMINAL_AdvStr("encoder_calibrate\r\n");

    //     self->mcu->DisableNvicInterrupt();

    //     // self->mcu->param_uart_baudrate = self->mcu->param_uart_baudrate_encoder_calibration;
    //     self->mcu->InitUart(self->mcu);

    //     // TERMINAL_AdvStr("----[CALIBRATION] Encoder calibration enable\r\n");

    //     self->init_flag.bit.encoder_calibration = self->EncoderCalibration(self, 1);

    //     self->driver_board->param_pwm_output_en = 0;
    //     self->controller->param_en_all_control_switch = 0;

    //     self->mcu->EnableNvicInterrupt();
    // }
}

__attribute__((section(".highcode"))) void CmdFunction_encoder_calibrate_official(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    // DBprintf("%s func\r\n", cmd_and_arg[0]);

    if (self->communication_interface.is_hex_frame == TRUE)
    {
        self->mcu->DisableNvicInterrupt();

        // TERMINAL_AdvStr("encoder_calibrate_official\r\n");

        /* motor phase check */
        self->CheckMotorPhaseOrder(self, 0);

        /* elec_offset_calibrate  */
        // self->mcu->param_uart_baudrate = self->mcu->param_uart_baudrate_encoder_calibration;
        // self->mcu->InitUart(self->mcu);
        // DBprintf("----[CALIBRATION] Elec_Zero_Offset calibration enable\r\n");

        /* encoder calibrate official */
        self->EncoderCalibrationOfficial_Run(self, 0, ResultOutputFormat_Hex);

        self->driver_board->param_pwm_output_en = 0;
        self->controller->param_en_all_control_switch = 0;

        // self->flash_manager_param->LoadAllVariableFromFlash(self->flash_manager_param);

        self->mcu->EnableNvicInterrupt();
    }
    else
    {
        self->mcu->DisableNvicInterrupt();

        // TERMINAL_AdvStr("encoder_calibrate_official\r\n");

        /* motor phase check */
        self->CheckMotorPhaseOrder(self, 1);

        /* elec_offset_calibrate  */
        // self->mcu->param_uart_baudrate = self->mcu->param_uart_baudrate_encoder_calibration;
        // self->mcu->InitUart(self->mcu);
        // DBprintf("----[CALIBRATION] Elec_Zero_Offset calibration enable\r\n");

        /* encoder calibrate official */
        self->EncoderCalibrationOfficial_Run(self, 1, ResultOutputFormat_Str);

        self->driver_board->param_pwm_output_en = 0;
        self->controller->param_en_all_control_switch = 0;

        // self->flash_manager_param->LoadAllVariableFromFlash(self->flash_manager_param);

        self->mcu->EnableNvicInterrupt();
    }
}

__attribute__((section(".highcode"))) void CmdFunction_elec_offset_calibrate(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    // DBprintf("%s func\r\n", cmd_and_arg[0]);

    if (self->communication_interface.is_hex_frame == TRUE)
    {
        switch (self->communication_interface.data_info.data[0])
        {
        case FunctionCmdCode2st_elec_offset_calibrate_null:
            self->mcu->DisableNvicInterrupt();

            // TERMINAL_AdvStr("elec_offset_calibrate\r\n");

            /* motor phase check */
            self->CheckMotorPhaseOrder(self, 0);

            /* elec_offset_calibrate  */
            // DBprintf("----[CALIBRATION] Elec_Zero_Offset calibration enable\r\n");
            self->ElecZeroOffsetCalibration(self, 0, ResultOutputFormat_Hex);

            self->driver_board->param_pwm_output_en = 0;
            self->controller->param_en_all_control_switch = 0;
            self->flash_manager_param->LoadAllVariableFromFlash(self->flash_manager_param);
            // DBprintf("----done, please reboot\r\n");

            self->mcu->EnableNvicInterrupt();
            break;
        }
    }
    else
    {
        self->mcu->DisableNvicInterrupt();

        // TERMINAL_AdvStr("elec_offset_calibrate\r\n");

        /* motor phase check */
        self->CheckMotorPhaseOrder(self, 1);

        /* elec_offset_calibrate  */
        // DBprintf("----[CALIBRATION] Elec_Zero_Offset calibration enable\r\n");
        self->ElecZeroOffsetCalibration(self, 1, ResultOutputFormat_Str);

        self->driver_board->param_pwm_output_en = 0;
        self->controller->param_en_all_control_switch = 0;
        self->flash_manager_param->LoadAllVariableFromFlash(self->flash_manager_param);
        // DBprintf("----done, please reboot\r\n");

        self->mcu->EnableNvicInterrupt();
    }
}

__attribute__((section(".highcode"))) void CmdFunction_control(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];
    // uint8_t data_len = raw_data[4];
    // uint8_t *data = &raw_data[5];

    static uint16_t en = 0;
    // DBprintf("%s func\r\n", cmd_and_arg[0]);

    if (self->communication_interface.is_hex_frame == TRUE)
    {
        switch (self->communication_interface.data_info.data[0])
        {
        case FunctionCmdCode2st_control_en_0:
            self->DriverEnableControl(self, 0);
            break;
        case FunctionCmdCode2st_control_en_1:
            self->DriverEnableControl(self, 1);
            break;
        case FunctionCmdCode2st_control_toggle:
            en = !en;
            if (en != 0)
            {
                self->DriverEnableControl(self, 1);
            }
            else
            {
                self->DriverEnableControl(self, 0);
            }
            break;
        case FunctionCmdCode2st_control_status:
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (uint8_t)self->step_controller.en_pin);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (uint8_t)self->controller->param_en_all_control_switch);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (uint8_t)self->driver_board->param_pwm_output_en);

            self->SendFormatedArray(self);
            break;
        default:
            break;
        }
    }
    else
    {
        if (strcmp((const char *)cmd_and_arg[1], "en") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "0") == 0)
            {
                self->DriverEnableControl(self, 0);
            }
            else if (strcmp((const char *)cmd_and_arg[2], "1") == 0)
            {
                self->DriverEnableControl(self, 1);
            }
        }
        else if (strcmp((const char *)cmd_and_arg[1], "toggle") == 0)
        {
            static uint16_t en = 0;
            en = !en;
            if (en != 0)
            {
                self->DriverEnableControl(self, 1);
            }
            else
            {
                self->DriverEnableControl(self, 0);
            }
        }
        else if (strcmp((const char *)cmd_and_arg[1], "status") == 0)
        {
            TERMINAL_Adv("en_pin_level: %d\r\n", self->step_controller.en_pin);
            TERMINAL_Adv("control_switch: %d\r\n", self->controller->param_en_all_control_switch);
            TERMINAL_Adv("pwm_output_en: %d\r\n", self->driver_board->param_pwm_output_en);
        }
    }
}

__attribute__((section(".highcode"))) void CmdFunction_sys_param(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    // DBprintf("%s func\r\n", cmd_and_arg[0]);

    if (self->communication_interface.is_hex_frame == TRUE)
    {
    }
    else
    {
        if (strcmp((const char *)cmd_and_arg[1], "read") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "all") == 0)
            {
                self->flash_manager_param->ListAllVariableOfSystemAsync_Trigger(self->flash_manager_param);
            }
            else
            {
                self->flash_manager_param->ListVariableOfSystem(self->flash_manager_param, cmd_and_arg[2]);
            }
        }
        else if (strcmp((const char *)cmd_and_arg[1], "write") == 0)
        {
        }
    }
}

__attribute__((section(".highcode"))) void CmdFunction_flash_param(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    // DBprintf("%s func\r\n", cmd_and_arg[0]);

    char str_tmp[10];

    if (self->communication_interface.is_hex_frame == TRUE)
    {
        switch (self->communication_interface.data_info.data[0])
        {
        case FunctionCmdCode2st_flash_param_null:
            break;
        case FunctionCmdCode2st_flash_param_reset:
            self->param_use_default_param = 1;
            self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "use_default_param");
            self->UpdateParam(self);
            self->SystemReboot(self);
            break;
        case FunctionCmdCode2st_flash_param_check:
            if (self->flash_manager_param->CheckAllDataValid(self->flash_manager_param))
            {
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, 0x01);
            }
            else
            {
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, 0x00);
            }
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (uint8_t)(self->flash_manager_param->checksum >> 0) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (uint8_t)(self->flash_manager_param->checksum >> 8) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (uint8_t)(self->flash_manager_param->flash_checksum >> 0) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (uint8_t)(self->flash_manager_param->flash_checksum >> 8) & 0xff);
            self->SendFormatedArray(self);
            break;
        case FunctionCmdCode2st_flash_param_save:
            self->flash_manager_param->SaveAllVariableIntoFlash(self->flash_manager_param);
            break;
        case FunctionCmdCode2st_flash_param_load:
            self->flash_manager_param->LoadAllVariableFromFlash(self->flash_manager_param);
            break;
        case FunctionCmdCode2st_flash_param_write:
            if (strcmp("uint16_t", self->flash_manager_param->data_table[self->communication_interface.data_info.data[1]].type) == 0)
            {
                data_conversion.u16[0] = ((uint16_t)self->communication_interface.data_info.data[3] << 8) | self->communication_interface.data_info.data[2];
                sprintf(str_tmp, "%d", data_conversion.u16[0]);
            }
            else if (strcmp("int16_t", self->flash_manager_param->data_table[self->communication_interface.data_info.data[1]].type) == 0)
            {
                data_conversion.u16[0] = ((uint16_t)self->communication_interface.data_info.data[3] << 8) | self->communication_interface.data_info.data[2];
                sprintf(str_tmp, "%d", data_conversion.s16[0]);
            }
            else if (strcmp("uint32_t", self->flash_manager_param->data_table[self->communication_interface.data_info.data[1]].type) == 0)
            {
                data_conversion.u16[0] = ((uint16_t)self->communication_interface.data_info.data[3] << 8) | self->communication_interface.data_info.data[2];
                data_conversion.u16[1] = ((uint16_t)self->communication_interface.data_info.data[5] << 8) | self->communication_interface.data_info.data[4];
                sprintf(str_tmp, "%d", data_conversion.u32);
            }
            else if (strcmp("int32_t", self->flash_manager_param->data_table[self->communication_interface.data_info.data[1]].type) == 0)
            {
                data_conversion.u16[0] = ((uint16_t)self->communication_interface.data_info.data[3] << 8) | self->communication_interface.data_info.data[2];
                data_conversion.u16[1] = ((uint16_t)self->communication_interface.data_info.data[5] << 8) | self->communication_interface.data_info.data[4];
                sprintf(str_tmp, "%d", data_conversion.s32);
            }
            else if (strcmp("float", self->flash_manager_param->data_table[self->communication_interface.data_info.data[1]].type) == 0)
            {
                data_conversion.u16[0] = ((uint16_t)self->communication_interface.data_info.data[3] << 8) | self->communication_interface.data_info.data[2];
                data_conversion.u16[1] = ((uint16_t)self->communication_interface.data_info.data[5] << 8) | self->communication_interface.data_info.data[4];
                sprintf(str_tmp, "%s", float2str(data_conversion.f32, 6));
            }

            self->DriverEnableControl(self, 0);
            self->flash_manager_param->WriteVariableIntoFlash(self->flash_manager_param, self->flash_manager_param->data_table[self->communication_interface.data_info.data[1]].name, str_tmp);
            self->flash_manager_param->LoadAllVariableFromFlash(self->flash_manager_param);
            self->UpdateParam(self);

            if (self->protection.err_code == 0)
            {
                self->DriverEnableControl(self, 1);
            }

            if (self->flash_manager_param->out_reboot_to_take_effect == FlashParam_RebootToTakeEffect_Yes)
            {
                self->SystemReboot(self);
            }
            break;
        case FunctionCmdCode2st_flash_param_read:
            self->flash_manager_param->ReadVariableFromFlash(self->flash_manager_param, self->flash_manager_param->data_table[self->communication_interface.data_info.data[1]].name);
            if (strcmp("uint16_t", self->flash_manager_param->data_table[self->communication_interface.data_info.data[1]].type) == 0 ||
                strcmp("int16_t", self->flash_manager_param->data_table[self->communication_interface.data_info.data[1]].type) == 0)
            {
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u16[0] >> 0) & 0xff);
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u16[0] >> 8) & 0xff);
            }
            else if (strcmp("uint32_t", self->flash_manager_param->data_table[self->communication_interface.data_info.data[1]].type) == 0 ||
                     strcmp("int32_t", self->flash_manager_param->data_table[self->communication_interface.data_info.data[1]].type) == 0 ||
                     strcmp("float", self->flash_manager_param->data_table[self->communication_interface.data_info.data[1]].type) == 0)
            {
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u16[0] >> 0) & 0xff);
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u16[0] >> 8) & 0xff);
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u16[1] >> 0) & 0xff);
                self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u16[1] >> 8) & 0xff);
            }
            self->SendFormatedArray(self);
            break;
        }
    }
    else
    {
        if (strcmp((const char *)cmd_and_arg[1], "reset") == 0)
        {
            self->DriverEnableControl(self, 0);
            self->param_use_default_param = 1;
            self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "use_default_param");
            self->UpdateParam(self);
            TERMINAL("\r\nRebooting system...\r\n");
            self->SystemReboot(self);
        }
        else if (strcmp((const char *)cmd_and_arg[1], "check") == 0)
        {
            if (self->flash_manager_param->CheckAllDataValid(self->flash_manager_param))
            {
                TERMINAL_Adv("[%d | %d]: ", self->flash_manager_param->flash_checksum, self->flash_manager_param->checksum);
                TERMINAL_AdvStr("flash check pass!\r\n");
            }
            else
            {
                TERMINAL_Adv("[%d | %d]: ", self->flash_manager_param->flash_checksum, self->flash_manager_param->checksum);
                TERMINAL_AdvStr("flash check failed!\r\n");
            }
        }
        else if (strcmp((const char *)cmd_and_arg[1], "save") == 0)
        {
            self->DriverEnableControl(self, 0);
            if (strcmp((const char *)cmd_and_arg[2], "all") == 0)
            {
                self->flash_manager_param->SaveAllVariableIntoFlash(self->flash_manager_param);
            }
            else
            {
                self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, cmd_and_arg[2]);
            }
            self->DriverEnableControl(self, 1);
            TERMINAL_AdvStr("save done.\r\n");
        }
        else if (strcmp((const char *)cmd_and_arg[1], "load") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "all") == 0)
            {
                self->flash_manager_param->LoadAllVariableFromFlash(self->flash_manager_param);
            }
            else
            {
                self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, cmd_and_arg[2]);
            }
            self->UpdateParam(self);
            TERMINAL_AdvStr("load done.\r\n");
        }
        else if (strcmp((const char *)cmd_and_arg[1], "write") == 0)
        {
            self->DriverEnableControl(self, 0);
            self->flash_manager_param->WriteVariableIntoFlash(self->flash_manager_param, cmd_and_arg[2], cmd_and_arg[3]);
            self->flash_manager_param->LoadAllVariableFromFlash(self->flash_manager_param);
            self->UpdateParam(self);
            TERMINAL_AdvStr("write done.\r\n");

            if (self->protection.err_code == 0)
            {
                self->DriverEnableControl(self, 1);
            }

            if (self->flash_manager_param->out_reboot_to_take_effect == FlashParam_RebootToTakeEffect_Yes)
            {
                self->SystemReboot(self);
            }
        }
        else if (strcmp((const char *)cmd_and_arg[1], "read") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "all") == 0)
            {
                self->flash_manager_param->ReadAllVariableFromFlashAsync_Trigger(self->flash_manager_param);
            }
            else
            {
                self->flash_manager_param->ReadVariableFromFlash(self->flash_manager_param, cmd_and_arg[2]);
            }
        }
    }
}

__attribute__((section(".highcode"))) void CmdFunction_get(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    // DBprintf("%s func\r\n", cmd_and_arg[0]);

    if (self->communication_interface.is_hex_frame == TRUE)
    {
        switch (self->communication_interface.data_info.data[0])
        {
        case FunctionCmdCode2st_get_null:
            data_conversion.f32 = 0;
            break;
        case FunctionCmdCode2st_get_pos_ref:
            data_conversion.f32 = self->controller->pos_loop_pid.in_ref;
            break;
        case FunctionCmdCode2st_get_spd_ref:
            data_conversion.f32 = self->controller->spd_loop_pid.in_ref;
            break;
        case FunctionCmdCode2st_get_id_ref:
            data_conversion.f32 = self->controller->id_loop_pid.in_ref;
            break;
        case FunctionCmdCode2st_get_iq_ref:
            data_conversion.f32 = self->controller->iq_loop_pid.in_ref;
            break;
        case FunctionCmdCode2st_get_pos_fdb:
            data_conversion.f32 = self->controller->pos_loop_pid.in_fdb;
            break;
        case FunctionCmdCode2st_get_spd_fdb:
            data_conversion.f32 = self->controller->spd_loop_pid.in_fdb;
            break;
        case FunctionCmdCode2st_get_id_fdb:
            data_conversion.f32 = self->controller->id_loop_pid.in_fdb;
            break;
        case FunctionCmdCode2st_get_iq_fdb:
            data_conversion.f32 = self->controller->iq_loop_pid.in_fdb;
            break;
        case FunctionCmdCode2st_get_ia_fdb:
            data_conversion.f32 = self->controller->in_ialpha_fdb;
            break;
        case FunctionCmdCode2st_get_ib_fdb:
            data_conversion.f32 = self->controller->in_ibeta_fdb;
            break;
        case FunctionCmdCode2st_get_steps:
            data_conversion.s32 = self->step_controller.step;
            break;
        case FunctionCmdCode2st_get_dir:
            data_conversion.u32 = self->step_controller.dir_pin;
            break;
        case FunctionCmdCode2st_get_en:
            data_conversion.u32 = self->step_controller.en_pin;
            break;
        case FunctionCmdCode2st_get_stall_is_trigger:
            data_conversion.u32 = self->stall_is_trigger;
            break;
        case FunctionCmdCode2st_get_encoder_fdb_singleturn:
            data_conversion.s32 = self->encoder->encoder_pulse_single_turn;
            break;
        case FunctionCmdCode2st_get_encoder_fdb_multiturn:
            data_conversion.s32 = self->encoder->encoder_pulse_multi_turns;
            break;
        case FunctionCmdCode2st_get_board_temp:
            data_conversion.f32 = self->driver_board->board_detect_temperature_C;
            break;
        case FunctionCmdCode2st_get_board_power:
            data_conversion.f32 = self->driver_board->board_detect_power_supply_V;
            break;
        }

        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 0) & 0xff);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 8) & 0xff);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 16) & 0xff);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (data_conversion.u32 >> 24) & 0xff);

        self->SendFormatedArray(self);
    }
    else
    {
        // if (strcmp((const char *)cmd_and_arg[1], "pulse") == 0)
        // {
        //     TERMINAL_Adv("%ld\r\n", (int32)(self->mcu->GetInputPulseCounter()));
        // }
        /* else */ if (strcmp((const char *)cmd_and_arg[1], "pos_fdb") == 0)
        {
            TERMINAL_Adv("%s\r\n", float2str(self->pos_fdb_encoder, 4));
        }
        else if (strcmp((const char *)cmd_and_arg[1], "spd_fdb") == 0)
        {
            TERMINAL_Adv("%s\r\n", float2str(self->controller->spd_fdb_ctrl, 4));
        }
        // else if (strcmp((const char *)cmd_and_arg[1], "idiq_fdb") == 0)
        // {
        //     TERMINAL_Adv("id:%s\r\n", float2str(self->controller->foc.id, 4));
        //     TERMINAL_Adv("iq:%s\r\n", float2str(self->controller->foc.iq, 4));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "pos_ref") == 0)
        // {
        //     TERMINAL_Adv("custom_pos_cmd:%s\r\n", float2str(self->controller->in_custom_pos_cmd, 4));
        //     TERMINAL_Adv("pos_ref:%s\r\n", float2str(self->controller->pos_loop_pid.in_ref, 4));
        //     TERMINAL_Adv("pos_fdb:%s\r\n", float2str(self->controller->pos_fdb_ctrl, 4));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "spd_ref") == 0)
        // {
        //     TERMINAL_Adv("custom_spd_cmd:%s\r\n", float2str(self->controller->in_custom_spd_cmd, 4));
        //     TERMINAL_Adv("spd_ref:%s\r\n", float2str(self->controller->spd_loop_pid.in_ref, 4));
        //     TERMINAL_Adv("spd_fdb:%s\r\n", float2str(self->controller->spd_fdb_ctrl, 4));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "idiq_ref") == 0)
        // {
        //     TERMINAL_Adv("custom_id_cmd:%s\t custom_iq_cmd:%s\r\n", float2str(self->controller->in_custom_id_cmd, 4), float2str(self->controller->in_custom_iq_cmd, 4));
        //     TERMINAL_Adv("idref:%s\t\t iqref:%s\r\n", float2str(self->controller->id_loop_pid.in_ref, 4), float2str(self->controller->iq_loop_pid.in_ref, 4));
        //     TERMINAL_Adv("idfdb:%s\t\t iqfdb:%s\r\n", float2str(self->controller->foc.id, 4), float2str(self->controller->foc.iq, 4));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "uduq_ref") == 0)
        // {
        //     TERMINAL_Adv("custom_ud_cmd:%s\t custom_uq_cmd:%s\r\n", float2str(self->controller->in_custom_ud_cmd, 4), float2str(self->controller->in_custom_uq_cmd, 4));
        //     TERMINAL_Adv("udref:%s\t\t uqref:%s\r\n", float2str(self->controller->foc.ud, 4), float2str(self->controller->foc.uq, 4));
        //     TERMINAL_Adv("idfdb:%s\t\t iqfdb:%s\r\n", float2str(self->controller->foc.id, 4), float2str(self->controller->foc.iq, 4));
        //     TERMINAL_Adv("custom_elecangle_cmd:%s\r\n", float2str(self->controller->in_custom_elecangle_cmd, 4));
        //     TERMINAL_Adv("foc.theta:%s\r\n", float2str(self->controller->foc.theta, 4));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "all_cmd") == 0)
        // {
        //     TERMINAL_Adv("pos_cmd:%s\r\n", float2str(self->in_pos_cmd, 4));
        //     TERMINAL_Adv("spd_cmd:%s\r\n", float2str(self->in_spd_cmd, 4));
        //     TERMINAL_Adv("acc_cmd:%s\r\n", float2str(self->in_acc_cmd, 4));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "ialphaibeta_fdb") == 0)
        // {
        //     TERMINAL_Adv("ialpha:%s\r\n", float2str(self->controller->foc.ialpha, 4));
        //     TERMINAL_Adv("ibeta:%s\r\n", float2str(self->controller->foc.ibeta, 4));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "uduq") == 0)
        // {
        //     TERMINAL_Adv("ud:%s\r\n", float2str(self->controller->foc.ud, 4));
        //     TERMINAL_Adv("uq:%s\r\n", float2str(self->controller->foc.uq, 4));
        // }
        else if (strcmp((const char *)cmd_and_arg[1], "encoder_fdb") == 0)
        {
            TERMINAL_Adv("%ld\r\n", self->encoder_fdb);
        }
        // else if (strcmp((const char *)cmd_and_arg[1], "singleturn") == 0)
        // {
        //     TERMINAL_Adv("%ld\r\n", self->encoder->encoder_pulse_single_turn);
        // }
        else if (strcmp((const char *)cmd_and_arg[1], "adc") == 0)
        {
            TERMINAL_Adv("IaIb value: %d %d\r\n", self->driver_board->Ia_adc_value, self->driver_board->Ib_adc_value);
            TERMINAL_Adv("IaIb offset: %d %d\r\n", self->driver_board->param_Ia_adc_offset, self->driver_board->param_Ib_adc_offset);
        }
        // else if (strcmp((const char *)cmd_and_arg[1], "phase_order") == 0)
        // {
        //     TERMINAL_Adv("phase_order: %d\r\n", self->param_phase_order_invert);
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "iqfdf_before_u") == 0)
        // {
        //     TERMINAL_Adv("iqfdf_before_u: %s\r\n", self->controller->iqfdf_before_u);
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "idiq_feedforward") == 0)
        // {
        //     TERMINAL_Adv("id_feedforward: %s\r\n", self->controller->id_feedforward);
        //     TERMINAL_Adv("iq_feedforward: %s\r\n", self->controller->iq_feedforward);
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "uduq_feedforward") == 0)
        // {
        //     TERMINAL_Adv("ud_feedforward: %s\r\n", float2str(self->controller->ud_feedforward, 3));
        //     TERMINAL_Adv("uq_feedforward: %s\r\n", float2str(self->controller->uq_feedforward, 3));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "foc.theta") == 0)
        // {
        //     TERMINAL_Adv("foc.theta: %s\r\n", float2str(self->controller->foc.theta, 3));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "fwc.out_iq_ref") == 0)
        // {
        //     TERMINAL_Adv("fwc.out_iq_ref: %s\r\n", float2str(self->controller->fwc.out_iq_ref, 3));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "iq_loop_pid.in_ref") == 0)
        // {
        //     TERMINAL_Adv("iq_loop_pid.in_ref: %s\r\n", float2str(self->controller->iq_loop_pid.in_ref, 3));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "spd_loop_pid.ref") == 0)
        // {
        //     TERMINAL_Adv("spd_loop_pid.ref: %s\r\n", float2str(self->controller->spd_loop_pid.in_ref, 3));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "spd_loop_pid.out") == 0)
        // {
        //     TERMINAL_Adv("spd_loop_pid.out: %s\r\n", float2str(self->controller->spd_loop_pid.out, 3));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "spd_feedforward") == 0)
        // {
        //     // TERMINAL_Adv("spd_fdf: %s\r\n", float2str(self->controller->spd_feedforward, 3));
        //     TERMINAL_Adv("%s %s %s %s\r\n",
        //                  float2str(self->controller->spd_fdb_ctrl, 4),
        //                  float2str(self->controller->spd_feedforward, 4),
        //                  float2str(self->controller->pos_loop_pid.out, 4),
        //                  float2str(self->controller->spd_loop_pid.in_ref, 4));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "leso") == 0)
        // {
        //     TERMINAL_Adv("leso.z1: %s\r\n", float2str(self->controller->leso.out_z1, 3));
        //     TERMINAL_Adv("leso.z2: %s\r\n", float2str(self->controller->leso.out_z2, 3));
        //     TERMINAL_Adv("leso.z3: %s\r\n", float2str(self->controller->leso.out_z3, 3));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "fwc") == 0)
        // {
        //     // TERMINAL_Adv("spd_fdf: %s\r\n", float2str(self->controller->spd_feedforward, 3));
        //     TERMINAL_Adv("fwc_id:%s\r\n", float2str(self->controller->fwc.out_id_ref, 4));
        //     TERMINAL_Adv("fwc_iq:%s\r\n", float2str(self->controller->fwc.out_iq_ref, 4));
        //     TERMINAL_Adv("iq_limit:%s\r\n", float2str(self->controller->fwc.iq_limit, 4));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "mech_offset") == 0)
        // {
        //     TERMINAL_Adv("mech_offset:%d\r\n", self->param_mech_offset);
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "steps") == 0)
        // {
        //     TERMINAL_Adv("step_turns:%ld ", self->step_controller.step_turns);
        //     TERMINAL_Adv("step:%ld\r\n", self->step_controller.step);
        //     TERMINAL_Adv("step_accumulate:%ld\r\n", self->step_controller.step_accumulate);
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "stall_is_trigger") == 0)
        // {
        //     TERMINAL_Adv("stall_is_trigger:%ld\r\n", self->stall_is_trigger);
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "power") == 0)
        // {
        //     TERMINAL_Adv("power(V):%s\r\n", float2str(self->driver_board->board_detect_power_supply_V, 4));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "temp") == 0)
        // {
        //     TERMINAL_Adv("temp(C):%s\r\n", float2str(self->driver_board->board_detect_temperature_C, 4));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "adc_ref") == 0)
        // {
        //     TERMINAL_Adv("ADC_refint:%d\r\n", self->mcu->AdcGetResult(ADC_NUM1, 3));
        //     TERMINAL_Adv("temp adc:%d\r\n", self->mcu->AdcGetResult(ADC_NUM1, 2));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "485_data_info") == 0)
        // {
        //     TERMINAL_Adv("addr:%#x\r\n", self->communication_interface.data_info.addr);
        //     TERMINAL_Adv("frame_len:%#x\r\n", self->communication_interface.data_info.frame_length);
        //     TERMINAL_Adv("status:%#x\r\n", self->communication_interface.data_info.status);
        //     TERMINAL_Adv("f_code:%#x\r\n", self->communication_interface.data_info.function_code);
        //     TERMINAL_Adv("data_len:%#x\r\n", self->communication_interface.data_info.data_len);
        //     TERMINAL_Adv("parity_r_c:%#x|%#x\r\n", self->communication_interface.data_info.parity_received, self->communication_interface.data_info.parity_cal);
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "zazen_pid") == 0)
        // {
        //     TERMINAL_Adv("pp: %s\r\n", float2str(self->controller->pos_loop_pid.kp, 4));
        //     TERMINAL_Adv("sp: %s\r\n", float2str(self->controller->spd_loop_pid.kp, 4));
        //     TERMINAL_Adv("si: %s\r\n", float2str(self->controller->spd_loop_pid.ki, 4));
        //     TERMINAL_Adv("d_p: %s\r\n", float2str(self->controller->id_loop_pid.kp, 4));
        //     TERMINAL_Adv("d_i: %s\r\n", float2str(self->controller->id_loop_pid.ki, 4));
        //     TERMINAL_Adv("q_p: %s\r\n", float2str(self->controller->iq_loop_pid.kp, 4));
        //     TERMINAL_Adv("q_i: %s\r\n", float2str(self->controller->iq_loop_pid.ki, 4));
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "erase") == 0)
        // {
        //     self->flash_manager_param->EraseSectionOfVariable(self->flash_manager_param, "zazen_gain_cur_kp");
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "savef") == 0)
        // {
        //     self->flash_manager_param->SaveSectionOfVariable(self->flash_manager_param, "zazen_gain_cur_kp");
        // }
        // else
        // {
        //     self->flash_manager_param->EraseAll(self->flash_manager_param);
        // }
    }
}

__attribute__((section(".highcode"))) void CmdFunction_logger(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    // DBprintf("%s func\r\n", cmd_and_arg[0]);

    // if (self->communication_interface.is_hex_frame == TRUE)
    // {
    //     switch (self->communication_interface.data_info.function_code)
    //     {
    //     case FunctionCmdCode2st_logger_en_0:
    //         self->logger->param_en = 0;
    //         break;
    //     case FunctionCmdCode2st_logger_en_1:
    //         self->logger->param_en = 1;
    //         break;
    //     case FunctionCmdCode2st_logger_continuous_mode_0:
    //         self->logger->param_continuous_send = 0;
    //         break;
    //     case FunctionCmdCode2st_logger_continuous_mode_1:
    //         self->logger->param_continuous_send = 1;
    //         break;
    //     case FunctionCmdCode2st_logger_trigger_0:
    //         self->logger->trigger = 0;
    //         break;
    //     case FunctionCmdCode2st_logger_trigger_1:
    //         self->logger->trigger = 1;
    //         break;
    //     default:
    //         break;
    //     }
    // }
    // else
    // {
    //     if (strcmp((const char *)cmd_and_arg[1], "en") == 0)
    //     {
    //         if (strcmp((const char *)cmd_and_arg[2], "0") == 0)
    //         {
    //             self->logger->param_en = 0;
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "1") == 0)
    //         {
    //             self->logger->param_en = 1;
    //         }
    //     }
    //     else if (strcmp((const char *)cmd_and_arg[1], "continuous_mode") == 0)
    //     {
    //         if (strcmp((const char *)cmd_and_arg[2], "0") == 0)
    //         {
    //             self->logger->param_continuous_send = 0;
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "1") == 0)
    //         {
    //             self->logger->param_continuous_send = 1;
    //         }
    //     }
    //     else if (strcmp((const char *)cmd_and_arg[1], "trigger") == 0)
    //     {
    //         if (strcmp((const char *)cmd_and_arg[2], "0") == 0)
    //         {
    //             self->logger->trigger = 0;
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "1") == 0)
    //         {
    //             self->logger->trigger = 1;
    //         }
    //     }
    // }
}

__attribute__((section(".highcode"))) void CmdFunction_ripple_cal(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    // DBprintf("%s func\r\n", cmd_and_arg[0]);

    if (self->communication_interface.is_hex_frame == TRUE)
    {
    }
    else
    {
        self->motor_ripple_table_calibrator.param_en = 1;
        self->motor_ripple_table_calibrator.procedure = MotorRippleTableCalibratorProgress_Init;
    }
}

__attribute__((section(".highcode"))) void CmdFunction_boot(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    // DBprintf("%s func\r\n", cmd_and_arg[0])

    char str_tmp[10];

    if (self->communication_interface.is_hex_frame == TRUE)
    {
        switch (self->communication_interface.data_info.data[0])
        {
        case FunctionCmdCode2st_boot_null:
            // sprintf(str_tmp, "%d", BOOT_ONCE_KEY);
            // // DBprintf("%s\r\n", str_tmp);
            // self->flash_manager_param->WriteVariableIntoFlash(self->flash_manager_param, "boot_key", str_tmp);
            // // self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "boot_key");

            // self->communication_interface.AddDataIntoSendArray(&self->communication_interface, 0x1);
            // self->SendFormatedArray(self);

            // self->SystemReboot(self);

            self->param_boot_key = BOOT_ONCE_KEY;
            // self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "boot_key");

            // self->communication_interface.AddDataIntoSendArray(&self->communication_interface, 0x1);
            // self->SendFormatedArray(self);

            self->SystemReboot(self);
            break;
        }
    }
    else
    {
        // sprintf(str_tmp, "%d", BOOT_ONCE_KEY);
        // // DBprintf("%s\r\n", str_tmp);
        // self->flash_manager_param->WriteVariableIntoFlash(self->flash_manager_param, "boot_key", str_tmp);
        // // self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "boot_key");
        // TERMINAL_AdvStr("\r\nok\r\n");
        // self->SystemReboot(self);

        self->param_boot_key = BOOT_ONCE_KEY;
        // self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "boot_key");
        TERMINAL_AdvStr("\r\nok\r\n");
        self->SystemReboot(self);
    };
}

__attribute__((section(".highcode"))) void CmdFunction_protection(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    if (self->communication_interface.is_hex_frame == TRUE)
    {
        switch (self->communication_interface.data_info.data[0])
        {
        case FunctionCmdCode2st_protection_null:
            break;
        case FunctionCmdCode2st_protection_en_0:
            self->protection.param_en = 0;
            self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "protect_en");
            break;
        case FunctionCmdCode2st_protection_en_1:
            self->protection.param_en = 1;
            self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "protect_en");
            break;
        case FunctionCmdCode2st_protection_report_0:
            self->protection.param_enable_report = 0;
            self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "protect_report");
            break;
        case FunctionCmdCode2st_protection_report_1:
            self->protection.param_enable_report = 1;
            self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "protect_report");
            break;
        case FunctionCmdCode2st_protection_reset:
            self->protection.err_code_raw = 0;
            self->protection.err_code_last = 0;
            self->protection.err_code = 0;
            self->protection.warning_code_raw = 0;
            self->protection.warning_code_last = 0;
            self->protection.warning_code = 0;
            break;
        case FunctionCmdCode2st_protection_err_code_backup:
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code_backup >> 0) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code_backup >> 8) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code_backup >> 16) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code_backup >> 24) & 0xff);
            self->SendFormatedArray(self);
            break;
        case FunctionCmdCode2st_protection_err_code:
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code >> 0) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code >> 8) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code >> 16) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code >> 24) & 0xff);
            self->SendFormatedArray(self);
            if (self->param_emergency_pin_mode == Emergency_Pin_Mode_ProtectionSignal)
            {
                //                self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);
            }
            break;
        case FunctionCmdCode2st_protection_warning_code_backup:
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code_backup >> 0) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code_backup >> 8) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code_backup >> 16) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code_backup >> 24) & 0xff);
            self->SendFormatedArray(self);
            break;
        case FunctionCmdCode2st_protection_warning_code:
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code >> 0) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code >> 8) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code >> 16) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code >> 24) & 0xff);
            self->SendFormatedArray(self);
            if (self->param_emergency_pin_mode == Emergency_Pin_Mode_ProtectionSignal)
            {
                //                self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);
            }
            break;
        case FunctionCmdCode2st_protection_all_code_backup:
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code_backup >> 0) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code_backup >> 8) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code_backup >> 16) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code_backup >> 24) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code_backup >> 0) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code_backup >> 8) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code_backup >> 16) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code_backup >> 24) & 0xff);
            self->SendFormatedArray(self);
            break;
        case FunctionCmdCode2st_protection_all_code:
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code >> 0) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code >> 8) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code >> 16) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.err_code >> 24) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code >> 0) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code >> 8) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code >> 16) & 0xff);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, (self->protection.warning_code >> 24) & 0xff);
            self->SendFormatedArray(self);
            if (self->param_emergency_pin_mode == Emergency_Pin_Mode_ProtectionSignal)
            {
                // self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);
            }
            break;
        }
    }
    else
    {
        if (strcmp((const char *)cmd_and_arg[1], "en") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "0") == 0)
            {
                self->protection.param_en = 0;
                self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "protect_en");
            }
            else if (strcmp((const char *)cmd_and_arg[2], "1") == 0)
            {
                self->protection.param_en = 1;
                self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "protect_en");
            }
        }
        else if (strcmp((const char *)cmd_and_arg[1], "report") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "0") == 0)
            {
                self->protection.param_enable_report = 0;
                self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "protect_report");
            }
            else if (strcmp((const char *)cmd_and_arg[2], "1") == 0)
            {
                self->protection.param_enable_report = 1;
                self->flash_manager_param->SaveVariableIntoFlash(self->flash_manager_param, "protect_report");
            }
        }
        else if (strcmp((const char *)cmd_and_arg[1], "reset") == 0)
        {
            self->protection.err_code_raw = 0;
        }
        else if (strcmp((const char *)cmd_and_arg[1], "err_code_backup") == 0)
        {
            // self->flash_manager_param->LoadVariableFromFlash(self->flash_manager_param, "protect_err_code_backup");
            self->protection.ErrParsing(&self->protection, self->protection.err_code_backup);
            // self->protection.ErrParsing(&self->protection, 256);
        }
        else if (strcmp((const char *)cmd_and_arg[1], "err_code") == 0)
        {
            self->protection.ErrParsing(&self->protection, self->protection.err_code);
        }
    }
}

__attribute__((section(".highcode"))) void CmdFunction_systemid(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    self->system_id_RL.param_uin_series[0] = self->system_id_RL.param_uin_series[1];
    if (self->communication_interface.is_hex_frame == TRUE)
    {
        switch (self->communication_interface.data_info.data[0])
        {
        case FunctionCmdCode2st_systemid_RL_A:
            self->system_id_RL.start = 1;
            self->system_id_RL.in_debug_info_output = 0;
            self->system_id_RL.in_result_output_format = ResultOutputFormat_Hex;
            self->system_id_RL.in_phase_num = 0;
            break;
        case FunctionCmdCode2st_systemid_RL_B:
            self->system_id_RL.start = 1;
            self->system_id_RL.in_debug_info_output = 0;
            self->system_id_RL.in_result_output_format = ResultOutputFormat_Hex;
            self->system_id_RL.in_phase_num = 1;
            break;
        case FunctionCmdCode2st_systemid_RL_ALL_NoSaving:
            self->motor_phase_check.in_save_into_flash_after_run = 0;
            self->motor_phase_check.in_debug_info_output = 0;
            self->motor_phase_check.in_result_output_format = ResultOutputFormat_Hex;
            self->motor_phase_check.start = 1;
            break;
        case FunctionCmdCode2st_systemid_RL_ALL_Saving:
            self->motor_phase_check.in_save_into_flash_after_run = 1;
            self->motor_phase_check.in_debug_info_output = 0;
            self->motor_phase_check.in_result_output_format = ResultOutputFormat_Hex;
            self->motor_phase_check.start = 1;
            break;
        default:
            break;
        }
    }
    else
    {
        if (strcmp((const char *)cmd_and_arg[1], "RL") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "A") == 0)
            {
                self->system_id_RL.start = 1;
                self->system_id_RL.in_debug_info_output = 1;
                self->system_id_RL.in_result_output_format = ResultOutputFormat_Str;
                self->system_id_RL.in_phase_num = 0;
            }
            else if (strcmp((const char *)cmd_and_arg[2], "B") == 0)
            {
                self->system_id_RL.start = 1;
                self->system_id_RL.in_debug_info_output = 1;
                self->system_id_RL.in_result_output_format = ResultOutputFormat_Str;
                self->system_id_RL.in_phase_num = 1;
            }
            else if (strcmp((const char *)cmd_and_arg[2], "ALL") == 0)
            {
                if (strcmp((const char *)cmd_and_arg[3], "1") == 0)
                {
                    self->motor_phase_check.in_save_into_flash_after_run = 1;
                    self->motor_phase_check.in_debug_info_output = 1;
                    self->motor_phase_check.in_result_output_format = ResultOutputFormat_Str;
                    self->motor_phase_check.start = 1;
                }
                else if (strcmp((const char *)cmd_and_arg[3], "0") == 0)
                {
                    self->motor_phase_check.in_save_into_flash_after_run = 0;
                    self->motor_phase_check.in_debug_info_output = 1;
                    self->motor_phase_check.in_result_output_format = ResultOutputFormat_Str;
                    self->motor_phase_check.start = 1;
                }
            }
        }
    }
}

__attribute__((section(".highcode"))) void CmdFunction_rs485_addr(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    if (self->communication_interface.is_hex_frame == TRUE)
    {
        switch (self->communication_interface.data_info.data[0])
        {
        case FunctionCmdCode2st_rs485_addr_set:
            // self->mcu->GpioPinInit(self->mcu, self->driver_board->param_pin_EMERGENCY.pin, GPIO_GENERAL_MODE_IN_PULL_UP);

            // self->rs485_addr = SlaveAddr_Base +
            //                    ((self->mcu->GpioRead(self->driver_board->param_pin_DIR.pin) << 2) |
            //                     (self->mcu->GpioRead(self->driver_board->param_pin_STEP.pin) << 1) |
            //                     (self->mcu->GpioRead(self->driver_board->param_pin_EMERGENCY.pin) << 0));
            // self->rs485_addr = SlaveAddr_Base +
            //                    ((self->mcu->GpioRead(self->driver_board->param_pin_STEP.pin) << 1) |
            //                     (self->mcu->GpioRead(self->driver_board->param_pin_DIR.pin) << 0));

            // self->mcu->GpioPinInit(self->mcu, self->driver_board->param_pin_EMERGENCY.pin, self->driver_board->param_pin_EMERGENCY.mode);
            break;
        case FunctionCmdCode2st_rs485_addr_get:
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->rs485_addr);

            self->SendFormatedArray(self);
            break;
        default:
            break;
        }
    }
    else
    {
        if (strcmp((const char *)cmd_and_arg[1], "set") == 0)
        {
            //            self->mcu->GpioPinInit(self->mcu, self->driver_board->param_pin_EMERGENCY.pin, GPIO_GENERAL_MODE_IN_PULL_UP);
            //
            //            DBprintf("%d\r\n", self->mcu->GpioRead(self->driver_board->param_pin_EMERGENCY.pin));
            //
            //            // self->rs485_addr = SlaveAddr_Base +
            //            //                    ((self->mcu->GpioRead(self->driver_board->param_pin_DIR.pin) << 2) |
            //            //                     (self->mcu->GpioRead(self->driver_board->param_pin_STEP.pin) << 1) |
            //            //                     (self->mcu->GpioRead(self->driver_board->param_pin_EMERGENCY.pin) << 0));
            //            self->rs485_addr = SlaveAddr_Base +
            //                               ((self->mcu->GpioRead(self->driver_board->param_pin_STEP.pin) << 1) |
            //                                (self->mcu->GpioRead(self->driver_board->param_pin_DIR.pin) << 0));
            //
            //            self->mcu->GpioPinInit(self->mcu, self->driver_board->param_pin_EMERGENCY.pin, self->driver_board->param_pin_EMERGENCY.mode);

        } //
        else if (strcmp((const char *)cmd_and_arg[1], "get") == 0)
        {
            TERMINAL_AdvStr("rs485_addr: ");
            TERMINAL_Adv("%#x\r\n", self->rs485_addr);
        }
    }
}

__attribute__((section(".highcode"))) void CmdFunction_version(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    if (self->communication_interface.is_hex_frame == TRUE)
    {
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->param_firmware_version.number.byte.major);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->param_firmware_version.number.byte.minor);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->param_firmware_version.number.byte.patch);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->param_firmware_version.number.byte.compiled);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->communication_interface.param_version.number.byte.major);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->communication_interface.param_version.number.byte.minor);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->protection.param_version.number.byte.major);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->protection.param_version.number.byte.minor);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->flash_manager_param->param_version.number.byte.major);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->flash_manager_param->param_version.number.byte.minor);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->logger->param_version.number.byte.major);
        self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->logger->param_version.number.byte.minor);

        self->SendFormatedArray(self);
    }
    else
    {
        if (strcmp((const char *)cmd_and_arg[1], "") == 0)
        {
            self->version_print_start = 1;
        }
        else if (strcmp((const char *)cmd_and_arg[1], "fireware") == 0)
        {
            TERMINAL_Adv("fireware ver: %d.%d.%d.%d | ", self->param_firmware_version.number.byte.major,
                         self->param_firmware_version.number.byte.minor,
                         self->param_firmware_version.number.byte.patch,
                         self->param_firmware_version.number.byte.compiled);
            TERMINAL_Adv(" %s\r\n", self->param_firmware_version.release_date);
        }
        else if (strcmp((const char *)cmd_and_arg[1], "cmd_system") == 0)
        {
            TERMINAL_Adv("cmd_system ver: %d.%d | ", self->communication_interface.param_version.number.byte.major,
                         self->communication_interface.param_version.number.byte.minor);
            TERMINAL_Adv(" %s\r\n", self->communication_interface.param_version.release_date);
        }
        else if (strcmp((const char *)cmd_and_arg[1], "protection") == 0)
        {
            TERMINAL_Adv("protection ver: %d.%d | ", self->protection.param_version.number.byte.major,
                         self->protection.param_version.number.byte.minor);
            TERMINAL_Adv(" %s\r\n", self->protection.param_version.release_date);
        }
        else if (strcmp((const char *)cmd_and_arg[1], "flash_param") == 0)
        {
            TERMINAL_Adv("flash_param ver: %d.%d | ", self->flash_manager_param->param_version.number.byte.major,
                         self->flash_manager_param->param_version.number.byte.minor);
            TERMINAL_Adv(" %s\r\n", self->flash_manager_param->param_version.release_date);
        }
        else if (strcmp((const char *)cmd_and_arg[1], "logger") == 0)
        {
            TERMINAL_Adv("logger ver: %d.%d | ", self->logger->param_version.number.byte.major,
                         self->logger->param_version.number.byte.minor);
            TERMINAL_Adv(" %s\r\n", self->logger->param_version.release_date);
        }
        else
        {
            /* code */
        }
    }
}

__attribute__((section(".highcode"))) void CmdFunction_control_loop(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    // if (self->communication_interface.is_hex_frame == TRUE)
    // {

    // }
    // else
    // {
    //     if (strcmp((const char *)cmd_and_arg[1], "mode") == 0)
    //     {
    //         if (strcmp((const char *)cmd_and_arg[2], "uduqloop_elec_angle_free") == 0)
    //         {
    //             DBprintf("mode: uduqloop_elec_angle_free\r\n");
    //             self->controller->param_control_mode = CONTROL_MODE_OPEN_LOOP_ELEC_ANGLE_FREE_CONTROL;
    //             self->controller->id_loop_pid.param_en = 0;
    //             self->controller->iq_loop_pid.param_en = 0;
    //             self->controller->spd_loop_pid.param_en = 0;
    //             self->controller->pos_loop_pid.param_en = 0;
    //             self->controller->id_loop_pid.Clear(&self->controller->id_loop_pid);
    //             self->controller->iq_loop_pid.Clear(&self->controller->iq_loop_pid);
    //             self->controller->spd_loop_pid.Clear(&self->controller->spd_loop_pid);
    //             self->controller->pos_loop_pid.Clear(&self->controller->pos_loop_pid);
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "uduqloop") == 0)
    //         {
    //             DBprintf("mode: uduqloop\r\n");
    //             self->controller->param_control_mode = CONTROL_MODE_OPEN_LOOP_CONTROL;
    //             self->controller->id_loop_pid.param_en = 0;
    //             self->controller->iq_loop_pid.param_en = 0;
    //             self->controller->spd_loop_pid.param_en = 0;
    //             self->controller->pos_loop_pid.param_en = 0;
    //             self->controller->id_loop_pid.Clear(&self->controller->id_loop_pid);
    //             self->controller->iq_loop_pid.Clear(&self->controller->iq_loop_pid);
    //             self->controller->spd_loop_pid.Clear(&self->controller->spd_loop_pid);
    //             self->controller->pos_loop_pid.Clear(&self->controller->pos_loop_pid);
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "curloop") == 0)
    //         {
    //             DBprintf("mode: curloop\r\n");
    //             self->controller->param_control_mode = CONTROL_MODE_CURRENT_CONTROL;
    //             self->controller->id_loop_pid.param_en = 1;
    //             self->controller->iq_loop_pid.param_en = 1;
    //             self->controller->spd_loop_pid.param_en = 0;
    //             self->controller->pos_loop_pid.param_en = 0;
    //             self->controller->id_loop_pid.Clear(&self->controller->id_loop_pid);
    //             self->controller->iq_loop_pid.Clear(&self->controller->iq_loop_pid);
    //             self->controller->spd_loop_pid.Clear(&self->controller->spd_loop_pid);
    //             self->controller->pos_loop_pid.Clear(&self->controller->pos_loop_pid);
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "spdloop") == 0)
    //         {
    //             DBprintf("mode: spdloop\r\n");
    //             self->controller->param_control_mode = CONTROL_MODE_SPEED_CONTROL;
    //             self->controller->id_loop_pid.param_en = 1;
    //             self->controller->iq_loop_pid.param_en = 1;
    //             self->controller->spd_loop_pid.param_en = 1;
    //             self->controller->pos_loop_pid.param_en = 0;
    //             self->controller->id_loop_pid.Clear(&self->controller->id_loop_pid);
    //             self->controller->iq_loop_pid.Clear(&self->controller->iq_loop_pid);
    //             self->controller->spd_loop_pid.Clear(&self->controller->spd_loop_pid);
    //             self->controller->pos_loop_pid.Clear(&self->controller->pos_loop_pid);
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "posloop") == 0)
    //         {
    //             DBprintf("mode: posloop\r\n");

    //             self->controller->param_control_mode = CONTROL_MODE_POSITION_CONTROL;
    //             self->step_controller.SetInitStepFromRad(&self->step_controller, self->controller->pos_fdb_ctrl);

    //             self->controller->id_loop_pid.param_en = 1;
    //             self->controller->iq_loop_pid.param_en = 1;
    //             self->controller->spd_loop_pid.param_en = 1;
    //             self->controller->pos_loop_pid.param_en = 1;
    //             self->controller->id_loop_pid.Clear(&self->controller->id_loop_pid);
    //             self->controller->iq_loop_pid.Clear(&self->controller->iq_loop_pid);
    //             self->controller->spd_loop_pid.Clear(&self->controller->spd_loop_pid);
    //             self->controller->pos_loop_pid.Clear(&self->controller->pos_loop_pid);
    //         }
    //     }
    //     else if (strcmp((const char *)cmd_and_arg[1], "add") == 0)
    //     {
    //         if (strcmp((const char *)cmd_and_arg[2], "elecangle") == 0)
    //         {
    //             if (strcmp((const char *)cmd_and_arg[3], "") != 0)
    //             {
    //                 self->controller->in_elec_angle_fdb += (float)atof(cmd_and_arg[3]);
    //                 TERMINAL_Adv("custom_elecangle_cmd:%s\r\n", float2str(self->controller->in_custom_elecangle_cmd, 4));
    //             }
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "uduq") == 0)
    //         {
    //             if (strcmp((const char *)cmd_and_arg[3], "") != 0 &&
    //                 strcmp((const char *)cmd_and_arg[4], "") != 0)
    //             {
    //                 self->controller->in_custom_ud_cmd += (float)atof(cmd_and_arg[3]);
    //                 self->controller->in_custom_uq_cmd += (float)atof(cmd_and_arg[4]);
    //                 TERMINAL_Adv("custom_ud_cmd:%s\t custom_uq_cmd:%s\r\n", float2str(self->controller->in_custom_ud_cmd, 4), float2str(self->controller->in_custom_uq_cmd, 4));
    //             }
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "idiqref") == 0)
    //         {
    //             if (strcmp((const char *)cmd_and_arg[3], "") != 0 &&
    //                 strcmp((const char *)cmd_and_arg[4], "") != 0)
    //             {
    //                 self->controller->in_custom_id_cmd += (float)atof(cmd_and_arg[3]);
    //                 self->controller->in_custom_iq_cmd += (float)atof(cmd_and_arg[4]);
    //                 TERMINAL_Adv("custom_id_cmd:%s\t custom_iq_cmd:%s\r\n", float2str(self->controller->in_custom_id_cmd, 4), float2str(self->controller->in_custom_iq_cmd, 4));
    //             }
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "spdref") == 0)
    //         {
    //             if (strcmp((const char *)cmd_and_arg[3], "") != 0)
    //             {
    //                 self->controller->in_custom_spd_cmd += (float)atof(cmd_and_arg[3]);
    //                 TERMINAL_Adv("custom_spd_cmd:%s\r\n", float2str(self->controller->in_custom_spd_cmd, 4));
    //             }
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "posref") == 0)
    //         {
    //             if (strcmp((const char *)cmd_and_arg[3], "") != 0)
    //             {
    //                 self->controller->in_custom_pos_cmd += (float)atof(cmd_and_arg[3]);
    //                 TERMINAL_Adv("custom_pos_cmd:%s\r\n", float2str(self->controller->in_custom_pos_cmd, 4));
    //             }
    //         }
    //     }
    //     else if (strcmp((const char *)cmd_and_arg[1], "set") == 0)
    //     {
    //         if (strcmp((const char *)cmd_and_arg[2], "elecangle") == 0)
    //         {
    //             if (strcmp((const char *)cmd_and_arg[3], "") != 0)
    //             {
    //                 self->controller->in_custom_elecangle_cmd = (float)atof(cmd_and_arg[3]);
    //                 TERMINAL_Adv("custom_elecangle_cmd:%s\r\n", float2str(self->controller->in_custom_elecangle_cmd, 4));
    //             }
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "uduq") == 0)
    //         {
    //             if (strcmp((const char *)cmd_and_arg[3], "") != 0 &&
    //                 strcmp((const char *)cmd_and_arg[4], "") != 0)
    //             {
    //                 self->controller->in_custom_ud_cmd = (float)atof(cmd_and_arg[3]);
    //                 self->controller->in_custom_uq_cmd = (float)atof(cmd_and_arg[4]);
    //                 TERMINAL_Adv("custom_ud_cmd:%s\t custom_uq_cmd:%s\r\n", float2str(self->controller->in_custom_ud_cmd, 4), float2str(self->controller->in_custom_uq_cmd, 4));
    //             }
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "idiqref") == 0)
    //         {
    //             if (strcmp((const char *)cmd_and_arg[3], "") != 0 &&
    //                 strcmp((const char *)cmd_and_arg[4], "") != 0)
    //             {
    //                 self->controller->in_custom_id_cmd = (float)atof(cmd_and_arg[3]);
    //                 self->controller->in_custom_iq_cmd = (float)atof(cmd_and_arg[4]);
    //                 TERMINAL_Adv("custom_id_cmd:%s\t custom_iq_cmd:%s\r\n", float2str(self->controller->in_custom_id_cmd, 4), float2str(self->controller->in_custom_iq_cmd, 4));
    //             }
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "spdref") == 0)
    //         {
    //             if (strcmp((const char *)cmd_and_arg[3], "") != 0)
    //             {
    //                 self->controller->in_custom_spd_cmd = (float)atof(cmd_and_arg[3]);
    //                 TERMINAL_Adv("custom_spd_cmd:%s\r\n", float2str(self->controller->in_custom_spd_cmd, 4));
    //             }
    //         }
    //         else if (strcmp((const char *)cmd_and_arg[2], "posref") == 0)
    //         {
    //             if (strcmp((const char *)cmd_and_arg[3], "") != 0)
    //             {
    //                 self->controller->in_custom_pos_cmd = (float)atof(cmd_and_arg[3]);
    //                 TERMINAL_Adv("custom_pos_cmd:%s\r\n", float2str(self->controller->in_custom_pos_cmd, 4));
    //             }
    //         }
    //     }
    // }
}

__attribute__((section(".highcode"))) void CmdFunction_emergency_pin_mode(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN])
{
    StepperMotorController *self = (StepperMotorController *)smc;
    // uint8_t *raw_data = (uint8_t *)(cmd_and_arg);
    // uint8_t function_code = raw_data[3];

    if (self->communication_interface.is_hex_frame == TRUE)
    {

        switch (self->communication_interface.data_info.data[0])
        {
        case FunctionCmdCode2st_emergency_pin_mode_null:
            break;
        case FunctionCmdCode2st_emergency_pin_mode_stall:
            self->param_emergency_pin_mode = Emergency_Pin_Mode_Stall;
            // self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, Emergency_Pin_Mode_Stall);
            self->SendFormatedArray(self);
            break;
        case FunctionCmdCode2st_emergency_pin_mode_protection:
            self->param_emergency_pin_mode = Emergency_Pin_Mode_ProtectionSignal;
            // self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, Emergency_Pin_Mode_ProtectionSignal);
            self->SendFormatedArray(self);
            break;
        case FunctionCmdCode2st_emergency_pin_mode_get:
            self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->param_emergency_pin_mode);
            // self->communication_interface.AddDataIntoSendArray(&self->communication_interface, self->mcu->GpioRead(self->driver_board->param_pin_EMERGENCY.pin));
            self->SendFormatedArray(self);
            break;
        }
    }
    else
    {
        // if (strcmp((const char *)cmd_and_arg[1], "1") == 0)
        // {
        //     self->param_emergency_pin_mode = Emergency_Pin_Mode_Stall;
        //     self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "2") == 0)
        // {
        //     self->param_emergency_pin_mode = Emergency_Pin_Mode_ProtectionSignal;
        //     self->mcu->GpioReset(self->driver_board->param_pin_EMERGENCY.pin);
        // }
        // else if (strcmp((const char *)cmd_and_arg[1], "get") == 0)
        // {
        //     TERMINAL_Adv("Emergency_pin_mode:%d\r\n", self->param_emergency_pin_mode);
        //     TERMINAL_Adv("EmergencyPinLevel:%d\r\n", self->mcu->GpioRead(self->driver_board->param_pin_EMERGENCY.pin));
        // }
        // else
        // {
        //     /* code */
        // }
    }
}
